Colony

//Obstacle Avoid Numbers

#ifndef _ORBIT_FSM_H_

#define _ORBIT_FSM_H_

//The States: 

#define ORBIT_SEEK      12           //Move straight towards robot

#define ORBIT_INSERTION 13           //Rotate into orbit

#define ORBIT_ORBITING  15           //Orbit robot

#define ORBIT_STOP      16           //Stop.  The default and ending state

#define ORBIT_STRAIGHT_SPEED 160

#define ORBIT_CORRECTION 9

#define ORBIT_DIRECTION 4     // if 4, then orbit to the right; if -4, then orbit to the left

#define ORBIT_DISTANCE 250 // orbit distance as measured by rangefinders

int orbit_state;    /*State machine variable.*/

int orbit_otherRobot; /* must be set prior to running FSM */

int orbit_pControl;		/*Proportional control variable, determines turn direction.*/

int orbit_d1,orbit_d2,orbit_d3,orbit_d4,orbit_d5;	/*The five distances taken in by IR.*/

int orbit_bom; /* bom data */

int orbit_theta; /* number of degrees traveled in orbit */

int orbit_theta_stop; /* stop condition in number of degrees traveled */

/* orbit_init

   argument: robot_id that you want to orbit

   notes: must call before orbit_fsm

*/

void orbit_init(int robot);

/* orbit_init_theta

   argument: robot_id that you want to orbit

             theta that you want to stop orbiting at

   notes: must call before orbit_fsm

*/

void orbit_init_theta(int robot,int theta);

/* orbit_fsm

   argument: none

   notes: call in a while loop to perform FSM action

*/

void orbit_fsm(void);

#endif

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file queue.h

 * @brief A queue implementation

 * 

 * Implements a queue, a first in, first out data structure.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#ifndef WIRELESS_QUEUE_H

#define WIRELESS_QUEUE_H

#ifndef ROBOT

#include <pthread.h>

#endif

struct node_def;

/**

 * @defgroup queue Queue

 * @brief A queue implementation

 * 

 * A queue implementation.

 *

 * @{

 **/

/**

 * @struct Queue

 * Represents a queue, a first in, first out data structure.

 **/

typedef struct

{

	/**

	 * The head of the queue, the next item to be removed.

	 **/

	struct node_def* head;

	/**

	 * The tail of the queue, the last item added.

	 **/

	struct node_def* tail;

	/**

	 * The number of elements in the queue.

	 **/

	int size;

#ifndef ROBOT

	pthread_mutex_t lock;

#endif

} Queue;

/** @brief Create a new queue **/

Queue* queue_create(void);

/** @brief Destroy a queue **/

void queue_destroy(Queue* q);

/** @brief Add an element to a queue **/

int queue_add(Queue* q, void* item);

/** @brief Remove an element from a queue **/

void* queue_remove(Queue* q);

/** @brief Remove all instances of a given element from a queue **/

void queue_remove_all(Queue* q, void* item);

/** @brief Get the size of a queue **/

int queue_size(Queue* q);

/** @brief Check if the queue is empty **/

int queue_is_empty(Queue* q);

/** @} **/

#endif

/** driver for orbit code

	execute orbit behavior

*/

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "bfs_fsm.h"

int main(void) {

  // enable everything

  dragonfly_init(ALL_ON);

  orb_enable();

  orb_init();

  orb_set_color(PURPLE);

  wl_init();

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  bfs_init(9); // set robot_id to find

  while(1) {

    bfs_fsm(); // do bfs

  }

  return 0;

}

//Obstacle Avoid Numbers

#ifndef _RUN_AROUND_FSM_H_

#define _RUN_AROUND_FSM_H_

//The States: 

#define MOVING 12           //Move strait.

#define BACKWARDS 15        //Move backwards. (Front close to wall.)

#define STOP 16             //Stop.  The default state, (Something broke).

#define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)

#define LEFT 37             //Left

#define RIGHT 39            //Right

#define BACKUP_MAX 20

#define CRAZY_MAX 200       //The number of counts between "crazy moments"

#define STRAIT_SPEED 160    //The speed when going strait or backing up.

#define TURN_CONSTANT 2

#define PCONTROL_CRAZY_LIMIT 80

int avoid_state;    /*State machine variable.*/

int crazy_count;    /*Counter for a 'get unstuck' behavior.*/

int backup_count;	/*Counter for backup duration.*/

int pControl;		/*Proportional control variable, determines turn direction.*/

int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/

void run_around_init(void);

void run_around_FSM(void);

#endif

########Update This Section########

#

#

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT = ../../..

endif

# Target file name (without extension).

TARGET = template

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = com7

#/dev/tty.usbserial*

#

#

###################################

# Hey Emacs, this is a -*- makefile -*-

#----------------------------------------------------------------------------

# WinAVR Makefile Template written by Eric B. Weddington, J?rg Wunsch, et al.

#

# Released to the Public Domain

#

# Additional material for this makefile was written by:

# Peter Fleury

# Tim Henigan

# Colin O'Flynn

# Reiner Patommel

# Markus Pfaff

# Sander Pool

# Frederik Rouleau

#

#----------------------------------------------------------------------------

# On command line:

#

# make all = Make software.

#

# make clean = Clean out built project files.

#

# make coff = Convert ELF to AVR COFF.

#

# make extcoff = Convert ELF to AVR Extended COFF.

#

# make program = Download the hex file to the device, using avrdude.

#                Please customize the avrdude settings below first!

#

# make debug = Start either simulavr or avarice as specified for debugging, 

#              with avr-gdb or avr-insight as the front end for debugging.

#

# make filename.s = Just compile filename.c into the assembler code only.

#

# make filename.i = Create a preprocessed source file for use in submitting

#                   bug reports to the GCC project.

#

# To rebuild project do "make clean" then "make all".

#----------------------------------------------------------------------------

#if you want your code to work on the Firefly++ and not Firefly+

#then add the -DFFPP line to CDEFS

CDEFS = 

#-DFFPP

# MCU name

MCU = atmega128

# Processor frequency.

#     This will define a symbol, F_CPU, in all source code files equal to the 

#     processor frequency. You can then use this symbol in your source code to 

#     calculate timings. Do NOT tack on a 'UL' at the end, this will be done

#     automatically to create a 32-bit value in your source code.

F_CPU = 8000000

# Output format. (can be srec, ihex, binary)

FORMAT = ihex

# List C source files here. (C dependencies are automatically generated.)

SRC = $(wildcard *.c)

# List Assembler source files here.

#     Make them always end in a capital .S.  Files ending in a lowercase .s

#     will not be considered source files but generated files (assembler

#     output from the compiler), and will be deleted upon "make clean"!

#     Even though the DOS/Win* filesystem matches both .s and .S the same,

#     it will preserve the spelling of the filenames, and gcc itself does

#     care about how the name is spelled on its command-line.

ASRC = 

# Optimization level, can be [0, 1, 2, 3, s]. 

#     0 = turn off optimization. s = optimize for size.

#     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)

OPT = s

# Debugging format.

#     Native formats for AVR-GCC's -g are dwarf-2 [default] or stabs.

#     AVR Studio 4.10 requires dwarf-2.

#     AVR [Extended] COFF format requires stabs, plus an avr-objcopy run.

DEBUG = 

# Compiler flag to set the C Standard level.

#     c89   = "ANSI" C

#     gnu89 = c89 plus GCC extensions

#     c99   = ISO C99 standard (not yet fully implemented)

#     gnu99 = c99 plus GCC extensions

CSTANDARD = -std=gnu99

# Place -D or -U options here

CDEFS += -DF_CPU=$(F_CPU)UL 

CDEFS += -DFFP

# for wireless library

ifdef USE_WIRELESS

	CDEFS += -DROBOT

endif

# Place -I, -L options here

CINCS = -I$(COLONYROOT)/code/lib/include/libdragonfly 

CINCS += -L$(COLONYROOT)/code/lib/bin

ifdef USE_WIRELESS

	CINCS += -I$(COLONYROOT)/code/lib/include/libwireless #-I$(COLONYROOT)/code/projects/colonet/lib -I$(COLONYROOT)/code/projects/colonet/lib/colonet_dragonfly

endif

#---------------- Compiler Options ----------------

#  -g*:          generate debugging information

#  -O*:          optimization level

#  -f...:        tuning, see GCC manual and avr-libc documentation

#  -Wall...:     warning level

#  -Wa,...:      tell GCC to pass this to the assembler.

#    -adhlns...: create assembler listing

CFLAGS =

# CFLAGS = -g$(DEBUG)

CFLAGS += $(CDEFS) $(CINCS)

CFLAGS += -O$(OPT)

CFLAGS += -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums

CFLAGS += -Wall -Wstrict-prototypes

CFLAGS += -Wa,-adhlns=$(<:.c=.lst)

CFLAGS += $(CSTANDARD)

#---------------- Assembler Options ----------------

#  -Wa,...:   tell GCC to pass this to the assembler.

#  -ahlms:    create listing

#  -gstabs:   have the assembler create line number information; note that

#             for use in COFF files, additional information about filenames

#             and function names needs to be present in the assembler source

#             files -- see avr-libc docs [FIXME: not yet described there]

ASFLAGS = -Wa,-adhlns=$(<:.S=.lst),-gstabs 

#---------------- Library Options ----------------

# Minimalistic printf version

PRINTF_LIB_MIN = -Wl,-u,vfprintf -lprintf_min

# Floating point printf version (requires MATH_LIB = -lm below)

PRINTF_LIB_FLOAT = -Wl,-u,vfprintf -lprintf_flt

# If this is left blank, then it will use the Standard printf version.

PRINTF_LIB = 

#PRINTF_LIB = $(PRINTF_LIB_MIN)

#PRINTF_LIB = $(PRINTF_LIB_FLOAT)

# Minimalistic scanf version

SCANF_LIB_MIN = -Wl,-u,vfscanf -lscanf_min

# Floating point + %[ scanf version (requires MATH_LIB = -lm below)

SCANF_LIB_FLOAT = -Wl,-u,vfscanf -lscanf_flt

# If this is left blank, then it will use the Standard scanf version.

SCANF_LIB = 

#SCANF_LIB = $(SCANF_LIB_MIN)

#SCANF_LIB = $(SCANF_LIB_FLOAT)

MATH_LIB = -lm

#---------------- External Memory Options ----------------

# 64 KB of external RAM, starting after internal RAM (ATmega128!),

# used for variables (.data/.bss) and heap (malloc()).

#EXTMEMOPTS = -Wl,-Tdata=0x801100,--defsym=__heap_end=0x80ffff

# 64 KB of external RAM, starting after internal RAM (ATmega128!),

# only used for heap (malloc()).

#EXTMEMOPTS = -Wl,--defsym=__heap_start=0x801100,--defsym=__heap_end=0x80ffff

EXTMEMOPTS =

#---------------- Linker Options ----------------

#  -Wl,...:     tell GCC to pass this to linker.

#    -Map:      create map file

#    --cref:    add cross reference to  map file

LDFLAGS = -Wl,-Map=$(TARGET).map,--cref

LDFLAGS += $(EXTMEMOPTS)

LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)

ifdef USE_WIRELESS

	LDFLAGS += -lwireless #-lcolonet_dragonfly

endif

LDFLAGS += -ldragonfly

#---------------- Programming Options (avrdude) ----------------

# Programming hardware: alf avr910 avrisp bascom bsd 

# dt006 pavr picoweb pony-stk200 sp12 stk200 stk500

#

# Type: avrdude -c ?

# to get a full listing.

#

AVRDUDE_PROGRAMMER = avrisp

# programmer connected to serial device

AVRDUDE_WRITE_FLASH = -b 57600 -U flash:w:$(TARGET).hex

#AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep

# Uncomment the following if you want avrdude's erase cycle counter.

# Note that this counter needs to be initialized first using -Yn,

# see avrdude manual.

#AVRDUDE_ERASE_COUNTER = -y

# Uncomment the following if you do /not/ wish a verification to be

# performed after programming the device.

#AVRDUDE_NO_VERIFY = -V

# Increase verbosity level.  Please use this when submitting bug

# reports about avrdude. See <http://savannah.nongnu.org/projects/avrdude> 

# to submit bug reports.

#AVRDUDE_VERBOSE = -v -v

AVRDUDE_FLAGS = -p $(MCU) -P $(AVRDUDE_PORT) -c $(AVRDUDE_PROGRAMMER)

AVRDUDE_FLAGS += $(AVRDUDE_NO_VERIFY)

AVRDUDE_FLAGS += $(AVRDUDE_VERBOSE)

AVRDUDE_FLAGS += $(AVRDUDE_ERASE_COUNTER)

#don't check for device signature

AVRDUDE_FLAGS += -F

#---------------- Debugging Options ----------------

# For simulavr only - target MCU frequency.

DEBUG_MFREQ = $(F_CPU)

# Set the DEBUG_UI to either gdb or insight.

# DEBUG_UI = gdb

DEBUG_UI = insight

# Set the debugging back-end to either avarice, simulavr.

DEBUG_BACKEND = avarice

#DEBUG_BACKEND = simulavr

# GDB Init Filename.

GDBINIT_FILE = __avr_gdbinit

# When using avarice settings for the JTAG

JTAG_DEV = /dev/com1

# Debugging port used to communicate between GDB / avarice / simulavr.

DEBUG_PORT = 4242

# Debugging host used to communicate between GDB / avarice / simulavr, normally

#     just set to localhost unless doing some sort of crazy debugging when 

#     avarice is running on a different computer.

DEBUG_HOST = localhost

#============================================================================

# Define programs and commands.

SHELL = sh

CC = avr-gcc

OBJCOPY = avr-objcopy

OBJDUMP = avr-objdump

SIZE = avr-size

NM = avr-nm

AVRDUDE = avrdude

REMOVE = rm -f

REMOVEDIR = rm -rf

COPY = cp

WINSHELL = cmd

# Define Messages

# English

MSG_ERRORS_NONE = Errors: none

MSG_BEGIN = -------- begin --------

MSG_END = --------  end  --------

MSG_SIZE_BEFORE = Size before: 

MSG_SIZE_AFTER = Size after:

MSG_COFF = Converting to AVR COFF:

MSG_EXTENDED_COFF = Converting to AVR Extended COFF:

MSG_FLASH = Creating load file for Flash:

MSG_EEPROM = Creating load file for EEPROM:

MSG_EXTENDED_LISTING = Creating Extended Listing:

MSG_SYMBOL_TABLE = Creating Symbol Table:

MSG_LINKING = Linking:

MSG_COMPILING = Compiling:

MSG_ASSEMBLING = Assembling:

MSG_CLEANING = Cleaning project:

# Define all object files.

OBJ = $(SRC:.c=.o) $(ASRC:.S=.o) 

# Define all listing files.

LST = $(SRC:.c=.lst) $(ASRC:.S=.lst) 

# Compiler flags to generate dependency files.

GENDEPFLAGS = -MD -MP -MF .dep/$(@F).d

# Combine all necessary flags and optional flags.

# Add target processor to flags.

ALL_CFLAGS = -mmcu=$(MCU) -I. $(CFLAGS) $(GENDEPFLAGS)

ALL_ASFLAGS = -mmcu=$(MCU) -I. -x assembler-with-cpp $(ASFLAGS)

# Default target.

all: begin gccversion sizebefore build sizeafter end

build: elf hex eep lss sym

elf: $(TARGET).elf

hex: $(TARGET).hex

eep: $(TARGET).eep

lss: $(TARGET).lss 

sym: $(TARGET).sym

# Eye candy.

# AVR Studio 3.x does not check make's exit code but relies on

# the following magic strings to be generated by the compile job.

begin:

	@echo

	@echo $(MSG_BEGIN)

end:

	@echo $(MSG_END)

	@echo

# Display size of file.

HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex

ELFSIZE = $(SIZE) -A $(TARGET).elf

AVRMEM = avr-mem.sh $(TARGET).elf $(MCU)

sizebefore:

	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_BEFORE); $(ELFSIZE); \

	$(AVRMEM) 2>/dev/null; echo; fi

sizeafter:

	@if test -f $(TARGET).elf; then echo; echo $(MSG_SIZE_AFTER); $(ELFSIZE); \

	$(AVRMEM) 2>/dev/null; echo; fi

# Display compiler version information.

gccversion : 

	@$(CC) --version

# Program the device.  

program: $(TARGET).hex $(TARGET).eep

	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)

# Generate avr-gdb config/init file which does the following:

#     define the reset signal, load the target file, connect to target, and set 

#     a breakpoint at main().

gdb-config: 

	@$(REMOVE) $(GDBINIT_FILE)

	@echo define reset >> $(GDBINIT_FILE)

	@echo SIGNAL SIGHUP >> $(GDBINIT_FILE)

	@echo end >> $(GDBINIT_FILE)

	@echo file $(TARGET).elf >> $(GDBINIT_FILE)

	@echo target remote $(DEBUG_HOST):$(DEBUG_PORT)  >> $(GDBINIT_FILE)

ifeq ($(DEBUG_BACKEND),simulavr)

	@echo load  >> $(GDBINIT_FILE)

endif	

	@echo break main >> $(GDBINIT_FILE)

debug: gdb-config $(TARGET).elf

ifeq ($(DEBUG_BACKEND), avarice)

	@echo Starting AVaRICE - Press enter when "waiting to connect" message displays.

	@$(WINSHELL) /c start avarice --jtag $(JTAG_DEV) --erase --program --file \

	$(TARGET).elf $(DEBUG_HOST):$(DEBUG_PORT)

	@$(WINSHELL) /c pause

else

	@$(WINSHELL) /c start simulavr --gdbserver --device $(MCU) --clock-freq \

	$(DEBUG_MFREQ) --port $(DEBUG_PORT)

endif

	@$(WINSHELL) /c start avr-$(DEBUG_UI) --command=$(GDBINIT_FILE)

# Convert ELF to COFF for use in debugging / simulating in AVR Studio or VMLAB.

COFFCONVERT=$(OBJCOPY) --debugging \

--change-section-address .data-0x800000 \

--change-section-address .bss-0x800000 \

--change-section-address .noinit-0x800000 \

--change-section-address .eeprom-0x810000 

coff: $(TARGET).elf

	@echo

	@echo $(MSG_COFF) $(TARGET).cof

	$(COFFCONVERT) -O coff-avr $< $(TARGET).cof

extcoff: $(TARGET).elf

	@echo

	@echo $(MSG_EXTENDED_COFF) $(TARGET).cof

	$(COFFCONVERT) -O coff-ext-avr $< $(TARGET).cof

# Create final output files (.hex, .eep) from ELF output file.

%.hex: %.elf

	@echo

	@echo $(MSG_FLASH) $@

	$(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@

%.eep: %.elf

	@echo

	@echo $(MSG_EEPROM) $@

	-$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \

	--change-section-lma .eeprom=0 -O $(FORMAT) $< $@

# Create extended listing file from ELF output file.

%.lss: %.elf

	@echo

	@echo $(MSG_EXTENDED_LISTING) $@

	$(OBJDUMP) -h -S $< > $@

# Create a symbol table from ELF output file.

%.sym: %.elf

	@echo

	@echo $(MSG_SYMBOL_TABLE) $@

	$(NM) -n $< > $@

# Link: create ELF output file from object files.

.SECONDARY : $(TARGET).elf

.PRECIOUS : $(OBJ)

%.elf: $(OBJ)

	@echo

	@echo $(MSG_LINKING) $@

	$(CC) $(ALL_CFLAGS) $^ --output $@ $(LDFLAGS)

# Compile: create object files from C source files.

%.o : %.c

	@echo

	@echo $(MSG_COMPILING) $<

	$(CC) -c $(ALL_CFLAGS) $< -o $@ 

# Compile: create assembler files from C source files.

%.s : %.c

	$(CC) -S $(ALL_CFLAGS) $< -o $@

# Assemble: create object files from assembler source files.

%.o : %.S

	@echo

	@echo $(MSG_ASSEMBLING) $<

	$(CC) -c $(ALL_ASFLAGS) $< -o $@

# Create preprocessed source for use in sending a bug report.

%.i : %.c

	$(CC) -E -mmcu=$(MCU) -I. $(CFLAGS) $< -o $@ 

# Target: clean project.

clean: begin clean_list end

clean_list :

	@echo

	@echo $(MSG_CLEANING)

	$(REMOVE) $(TARGET).hex

	$(REMOVE) $(TARGET).eep

	$(REMOVE) $(TARGET).cof

	$(REMOVE) $(TARGET).elf

	$(REMOVE) $(TARGET).map

	$(REMOVE) $(TARGET).sym

	$(REMOVE) $(TARGET).lss

	$(REMOVE) $(OBJ)

	$(REMOVE) $(LST)

	$(REMOVE) $(SRC:.c=.s)

	$(REMOVE) $(SRC:.c=.d)

	$(REMOVEDIR) .dep

# Include the dependency files.

-include $(shell mkdir .dep 2>/dev/null) $(wildcard .dep/*)

# Listing of phony targets.

.PHONY : all begin finish end sizebefore sizeafter gccversion \

build elf hex eep lss sym coff extcoff \

clean clean_list program debug gdb-config

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "queue.h"

#include <string.h>

#include "bfs_fsm.h"

#include "smart_run_around_fsm.h"

#include "orbit_fsm.h"

/* Used to find a robot (or other object)

  Uses bom and token ring

  Assumes:

    robots are already in a token ring

*/

/* private function prototypes */

void bfs_evaluate_state(void);

int bfs_follow(void);

/* init */

void bfs_init(int robot) {

  range_init();

  analog_init(1);

  motors_init();

  orb_init();

  orb_enable();

  usb_init();

  run_around_init();

  /*Start in the start state, BFS_SEEK */ 

  bfs_state = BFS_SEEK;

  bfs_otherRobot = robot; // set which robot to seek

  bfs_my_id = wl_get_xbee_id();

  bfs_follow_id = -1;

  bfs_pControl=0;

  bfs_bom = 0;

  bfs_bom_count = 0;

  /*Initialize distances to zero.*/ 

  bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000;

}

/*The main function, call this to update states as frequently as possible.*/

void bfs_fsm(void) {

  /*The following lines ensure that undefined (-1) values

  will not update the distances.*/ 

  int temp;  

  wl_do(); // update wireless

  usb_puts("\n\rwl_do");

  usb_puts("|state=");

  usb_puti(bfs_state);

  usb_puts("|");

  if (bfs_state == BFS_SEEK) {

    bfs_follow_id = bfs_follow();

    usb_puti(bfs_follow_id);

    if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) {

      bfs_state = BFS_FOLLOW; // move to follow state

      bfs_d2=1000;

    }

  }

  if (bfs_state == BFS_FOLLOW) {

    // get bom reading

    temp = wl_token_get_my_sensor_reading(bfs_follow_id);

    if (temp == -1 || temp > 16) {

      bfs_bom_count++;

      if (bfs_bom_count > BFS_MAX_BOM_COUNT) {

        bfs_state = BFS_SEEK;

        bfs_bom = 255;

        bfs_bom_count = 0;

      }

    }

    else {

      bfs_bom = temp;

      // modify bom reading so right is negative, left is positive

      if (bfs_bom <= 12)

        bfs_bom -= 4;

      else

        bfs_bom -= 20;

      bfs_pControl = bfs_bom*4;

      usb_puti(bfs_bom);

      // get range reading for front

      temp=range_read_distance(IR2);

      bfs_d2=(temp == -1) ? bfs_d2 : temp;

      usb_puti(bfs_d2);

      if (bfs_d2 < BFS_ORBIT_DISTANCE) {

        bfs_state = BFS_ORBIT; // move to orbit state

        orbit_init(bfs_follow_id);

        bfs_d2=1000;

      }

    }  

  }

  if (bfs_state == BFS_ORBIT) {   

    usb_puts("orbiting\n");

    while(orbit_state != ORBIT_STOP)

    { orbit_fsm(); }

    bfs_state = BFS_STOP; // move to stop state  

  }

  // evaluate state

  bfs_evaluate_state();

}

//Acts on state change.

void bfs_evaluate_state(){

    switch(bfs_state){

    case(BFS_SEEK): orb_set_color(RED);

      // move around

      run_around_FSM(); // note: better to just incorporate into this file later one

      break;

    case(BFS_FOLLOW): orb_set_color(GREEN);

      // follow another robot

      move(BFS_STRAIGHT_SPEED,bfs_pControl);

      break;

    case(BFS_ORBIT): //orb_set_color(BLUE);

      // orbit a robot

      //orbit_fsm();

      //move(0,0);

      break;

    case(BFS_STOP): orb_set_color(YELLOW);

      // stop

      move(0,0);

      break;

    default: orb_set_color(MAGENTA);

      /*Should never get here, so stop.*/

      move(0,0);

      break;

  }

}

/* find a robot to follow using BFS 

  ported from colonybfs by Felix

*/

int bfs_follow()

{

/* pseudocode for BFS

procedure bfs(v)

    q := make_queue()

    enqueue(q,v)

    mark v as visited

    while q is not empty

        v = dequeue(q)

        process v

        for all unvisited vertices v' adjacent to v

            mark v' as visited

            enqueue(q,v')

*/

  Queue* q = queue_create();

  //int num_current_robots = wl_token_get_robots_in_ring();

  // keep track of which nodes you have visited.  Indexed by robot #

  // 1 if visited, 0 if not

  unsigned char visited_nodes[BFS_MAX_ROBOTS];

  // keep track of the distance from the start robot to other robots

  // also indexed by robot#

  unsigned char node_distances[BFS_MAX_ROBOTS];

  // this is the path you take

  unsigned char path[BFS_MAX_ROBOTS];

  //variables you will need

  unsigned char current_node;                 //current node

  unsigned char current_neighbour_node;       //neighbor to the current node

  unsigned char current_neighbour_val;        //value of that neighbour's sensors

  unsigned char current_distance;              //keep track of current distance to the start

  unsigned char large_number = 255;

  unsigned char* add_value;

  //set visited nodes to all 0

  memset(visited_nodes, 0, BFS_MAX_ROBOTS);

  //set all the distances to a very large number

  //this isn't technically necessary, but it's probably a good thing -- just in case

  memset(node_distances, large_number, BFS_MAX_ROBOTS);

  //set the path to all LARGE_NUMBER as well

  memset(path, BFS_NO_VAL, BFS_MAX_ROBOTS);

  //queue_remove_all(q);

  //add the start node

  add_value = (unsigned char*)malloc(sizeof(char));

  (*add_value) = bfs_my_id;

  queue_add(q, add_value);

  visited_nodes[bfs_my_id] = 1;

  node_distances[bfs_my_id] = 0;

  while(!queue_is_empty(q)){

    add_value = queue_remove(q);

    current_node = (*add_value);

    //get the current distance from you to the start

    current_distance = node_distances[current_node];

    //this node is on your 'path'

    path[current_distance] = current_node;

    //note: it's OK if this path leads nowhere -- the path will be

    //overwritten by a node that does lead to the goal

    //(if there is no path, we set path to null anyways)

    //from now on, all further nodes will be one further away -- increment

    current_distance++;

    //process node -- basically check if it's the goal

    if(current_node == bfs_otherRobot) {

      //you reach the goal

      //return the first robot in the path, which is the one

      //to follow

      /*

      lcd_putchar('.');

      lcd_putchar('.');

      for(i = 0; i < MAX_ROBOTS; i++)

          lcd_putchar(path[i] +48);           //print out the path for fun -- remove this later

      lcd_putchar('.');

      lcd_putchar('.');

      */

      return path[1];         //path[0] is you.  path[1] is who you want to follow

    }

    // go through all nodes adjacent to current

    // in effect, this means going through the current node's row in the matrix

    wl_token_iterator_begin();

    while(wl_token_iterator_has_next()) {

      //the robot # is actually just the index

      current_neighbour_node = wl_token_iterator_next();

      //the value is what is stored in the matrix (this is important)

      current_neighbour_val = wl_token_get_sensor_reading(current_node,current_neighbour_node);

      //check for connected-ness and that it was not visited

      //            unconnected                           visited

      if( (current_neighbour_val == BFS_NO_VAL) || (visited_nodes[current_neighbour_node]) ) {

        //if it is either unconnected or previously visited, exit

        continue;   //go to the next node

      }

      //update the distance from the start node to this node

      node_distances[current_neighbour_node] = current_distance;

      //also mark it as visited

      visited_nodes[current_neighbour_node] = 1;

      //also enqueue each neighbour

      add_value = (unsigned char*)malloc(sizeof(char));

      (*add_value) = current_neighbour_node;

      queue_add(q, add_value);

    }

  }

  //if we get to here, there is no path

  memset(path, 0, BFS_MAX_ROBOTS);

  return BFS_NO_VAL;

}

#include <dragonfly_lib.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "orbit_fsm.h"

#include "bfs_fsm.h"

/* Used to orbit a robot

  Must be within BOM range of robot before activating

  orbit_theta_stop requires encoders, and is incomplete

  Assumes:

    both robots are already in a token ring

*/

/* private function prototype */

void orbit_evaluate_state(void);

/* primary init */

void orbit_init(int robot) {

  //range_init();

  //analog_init(1);

  //motors_init();

  //orb_init();

  //orb_enable();

  //usb_init();

  /*Start in the start state, ORBIT_SEEK */ 

  orbit_state = ORBIT_INSERTION;

  orbit_otherRobot = robot; // set which robot to seek and orbit

  orbit_pControl=0;

  orbit_bom = bfs_bom;

  orbit_theta = 0;

  orbit_theta_stop = 1000;

  /*Initialize distances to zero.*/ 

  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;

}

/* secondary init */

void orbit_init_theta(int robot,int theta) {

  //range_init();

  //analog_init(1);

  //motors_init();

  //orb_init();

  //orb_enable();

  //usb_init();

  /*Start in the start state, ORBIT_INSERTION */ 

  orbit_state = ORBIT_INSERTION;

  orbit_otherRobot = robot; // set which robot to seek and orbit

  orbit_pControl=0;

  orbit_bom = bfs_bom;

  orbit_theta = 0;

  orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop

  /*Initialize distances to zero.*/ 

  orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000;

}

/*The main function, call this to update states as frequently as possible.*/

void orbit_fsm(void) {

  /*The following lines ensure that undefined (-1) values

  will not update the distances.*/ 

  int temp;  

  //temp=range_read_distance(IR1);

  //orbit_d1=(temp == -1) ? orbit_d1 : temp;

  temp=range_read_distance(IR2);

  orbit_d2=(temp == -1) ? orbit_d2 : temp;

  //temp=range_read_distance(IR3);

  //orbit_d3=(temp == -1) ? orbit_d3 : temp;

  //temp=range_read_distance(IR4);

  //orbit_d4=(temp == -1) ? orbit_d4 : temp;

  //temp=range_read_distance(IR5);

  //orbit_d5=(temp == -1) ? orbit_d5 : temp;

  wl_do(); // update wireless

  // get bom reading

  temp = wl_token_get_my_sensor_reading(orbit_otherRobot);

  orbit_bom = (temp == -1) ? orbit_bom : temp;

  // modify bom reading so right is negative, left is positive

  if (orbit_bom <= 12)

    orbit_bom -= 4;

  else

    orbit_bom -= 20;

  if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE)

    orbit_state = ORBIT_INSERTION; // begin to orbit

  if (orbit_state == ORBIT_INSERTION &&

    (orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION))

    orbit_state = ORBIT_ORBITING; // orbit achieved

  if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_DISTANCE) 

    || (orbit_theta >= orbit_theta_stop)))

    orbit_state = ORBIT_STOP; // orbit obstructed

  // evaluate state

  orbit_evaluate_state();

}

//Acts on state change.

void orbit_evaluate_state(){

    switch(orbit_state){

    case(ORBIT_SEEK): orb_set_color(RED);

      // move towards robot

      orbit_pControl = orbit_bom*10;      

      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);

      break;

    case(ORBIT_INSERTION): orb_set_color(GREEN);

      // rotate into orbit, perpendicular to other robot

      orbit_pControl = -ORBIT_DIRECTION*10;

      move(ORBIT_STRAIGHT_SPEED/3,orbit_pControl);

      break;

    case(ORBIT_ORBITING): orb_set_color(BLUE);

      // go straight with slight rotation

      if (orbit_bom == ORBIT_DIRECTION)

        orbit_pControl = ORBIT_DIRECTION;

      else if (orbit_bom < ORBIT_DIRECTION)

        orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION;

      else

        orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION;

      move(ORBIT_STRAIGHT_SPEED,orbit_pControl);

      break;

    case(ORBIT_STOP): orb_set_color(YELLOW);

      move(0,0);

      break;

    default: orb_set_color(YELLOW);

      /*Should never get here, so stop.*/

      move(0,0);

      break;

  }

}

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file queue.c

 * @brief Queue Implementation

 *

 * An implementation of a queue for the colony robots.

 *

 * @author Brian Coltin, Colony Project, CMU Robotics Club

 **/

#include "queue.h"

#include <wl_defs.h>

#include <stdlib.h>

#include <stdio.h>

#ifndef ROBOT

#include <pthread.h>

#endif

/**

 * @struct node_def

 * A node in the queue.

 **/

typedef struct node_def

{

	/** The item at this point in the queue. **/

	void* val;

	/** The next node in the queue. **/

	struct node_def* next;

} Node;

/**

 * Create a queue.

 *

 * @return the newly created queue.

 **/

Queue* queue_create()

{

	Queue* q = (Queue*)malloc(sizeof(Queue));

	if (q == NULL)

	{

		WL_DEBUG_PRINT("Out of memory.\r\n");

		return NULL;

	}

	q->head = NULL;

	q->size = 0;

	#ifndef ROBOT

	pthread_mutex_init(&(q->lock), NULL);

	#endif

	return q;

}

/**

 * Destroys a queue, freeing memory.

 *

 * @param q the queue to destroy

 **/

void queue_destroy(Queue* q)

{

	Node* t = q->head;

	while (t != NULL)

	{

		Node* t2 = t->next;

		free(t);

		t = t2;

	}

	#ifndef ROBOT

	pthread_mutex_destroy(&(q->lock));

	#endif

	free(q);

}

/**

 * Add an element to a queue.

 *

 * @param q the queue to add an element to

 * @param item the item to add to the queue

 **/

int queue_add(Queue* q, void* item)

{

	#ifndef ROBOT

	pthread_mutex_lock(&(q->lock));

	#endif

	Node* n = (Node*)malloc(sizeof(Node));

	if (n == NULL)

	{

		#ifndef ROBOT

		pthread_mutex_unlock(&(q->lock));

		#endif

		return -1;

	}

	n->val = item;

	n->next = NULL;

	if (q->head == NULL)

	{

		q->head = n;

		q->tail = n;

	}

	else

	{

		q->tail->next = n;

		q->tail = n;

	}

	q->size++;

	#ifndef ROBOT

	pthread_mutex_unlock(&(q->lock));

	#endif

	return 0;

}

/**

 * Remove an element from the front of a queue.

 *

 * @param q the queue to remove the element from

 *

 * @return the element which was removed

 **/

void* queue_remove(Queue* q)

{

	#ifndef ROBOT

	pthread_mutex_lock(&(q->lock));

	#endif

	Node* first = q->head;

	if (first == NULL)

	{

		WL_DEBUG_PRINT("Attempting to remove item \

			from empty queue.\r\n");

		WL_DEBUG_PRINT_INT(queue_size(q));

		#ifndef ROBOT

		pthread_mutex_unlock(&(q->lock));

		#endif

		return NULL;

	}

	void* ret = first->val;

	q->head = first->next;

	if (q->tail == first)

		q->tail = NULL;

	free(first);

	q->size--;

	#ifndef ROBOT

	pthread_mutex_unlock(&(q->lock));

	#endif

	return ret;

}

/**

 * Remove all instances of a given element from a queue.

 *

 * @param q the queue to remove the elements from

 * @param item the element to remove all instances of

 **/

void queue_remove_all(Queue* q, void* item)

{

	#ifndef ROBOT

	pthread_mutex_lock(&(q->lock));

	#endif

	Node* curr = q->head;

	Node* prev = NULL;

	while (curr != NULL)

	{

		Node* next = curr->next;

		if (curr->val == item)

		{

			if (q->head == curr)

				q->head = next;

			if (q->tail == curr)

				q->tail = prev;

			if (prev != NULL)

				prev->next = next;

			free(curr);

			q->size--;

		}

		else

			prev = curr;

		curr = next;

	}

	#ifndef ROBOT

	pthread_mutex_unlock(&(q->lock));

	#endif

}

/**

 * Get the number of elements in the queue.

 *

 * @param q the queue to get the size of

 * @return the size of the queue

 **/

int queue_size(Queue* q)

{

	int size;

	#ifndef ROBOT

	pthread_mutex_lock(&(q->lock));

	#endif

	size = q->size;

	#ifndef ROBOT

	pthread_mutex_unlock(&(q->lock));

	#endif

	return size;

}

/**

 * Check if the queue is empty.

 * 

 * @param q the queue to check

 * @return nonzero if the queue is empty, 0 otherwise

 **/

int queue_is_empty(Queue* q)

{

	int result;

	#ifndef ROBOT

	pthread_mutex_lock(&(q->lock));

	#endif

	result = q->size == 0;

	#ifndef ROBOT

	pthread_mutex_unlock(&(q->lock));

	#endif

	return result;

}

// BFS FSM header file

#ifndef _BFS_FSM_H_

#define _BFS_FSM_H_

//The States: 

#define BFS_SEEK      12           //do run around

#define BFS_FOLLOW    13           //follow other robots to location

#define BFS_ORBIT     15           //Orbit robot

#define BFS_STOP      16           //Stop.  The default and ending state

#define BFS_STRAIGHT_SPEED 160