Colony - / - Diff - Robotics Club

Revision 1450

     #include <dragonfly_lib.h>
     #include <eeprom.h>
     int main()
+    {
       char str[5];
       int i=0,id;
       char c;
       dragonfly_init(ALL_ON);
       usb_puts("Here is the current setup:\r\n  ID: ");
       itoa(get_robotid(), str, 10);
       usb_puts(str);
       usb_puts("\r\n  BOM type: ");
       itoa(get_bom_type(), str, 10);
       usb_puts(str);
       usb_puts("\r\n\r\nEnter new ID:");
       while((c = usb_getc()) != '\n' && c != '\r')
+        {
           usb_putc(c);
           if(i>=4)
+    	{
     	  usb_puts("ERROR: filled buffer\n");
     	  while(1);
+    	}
           str[i++] = c;
+        }
       while(i<5)
         str[i++] = 0;
       id = atoi(str);
       usb_puts("\r\nsetting new id to: ");
       usb_puti(id);
       eeprom_put_byte(EEPROM_ROBOT_ID_ADDR, 'I');
       eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+1, 'D');
       eeprom_put_byte(EEPROM_ROBOT_ID_ADDR+2, id);
       usb_puts("\r\nEnter BOM type:\r\b"
     	   "1 for BOM 1.0\r\n"
     	   "5 for BOM 1.5\r\n"
     	   "r for RBOM\r\n>");
       c = usb_getc();
       usb_putc(c);
       switch(c)
+        {
         case '1': id = BOM10; break;
         case '5': id = BOM15; break;
         case 'r': id = RBOM; break;
         default:
           usb_puts("ERROR: invalid input");
           while(1);
+        }
       eeprom_put_byte(EEPROM_BOM_TYPE_ADDR, 'B');
       eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+1, 'O');
       eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+2, 'M');
       eeprom_put_byte(EEPROM_BOM_TYPE_ADDR+3, id);
       usb_puts("\r\ndone! have a nice day\r\n");
       while(1);
       return 0;
+    }

     ########Update This Section########
+    #
+    #
     # Relative path to the root directory (containing lib directory)
     ifndef COLONYROOT
     COLONYROOT = ../../..
     endif
     # Target file name (without extension).
     TARGET = program_eeprom
     # Uncomment this to use the wireless library
     USE_WIRELESS = 1
     # com1 = serial port. Use lpt1 to connect to parallel port.
     AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
+    #
+    #
     ###################################
     # 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
     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
     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 and rangefinders
       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) {
       /* reset some values */
       bfs_speed = BFS_STRAIGHT_SPEED;
       /*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("|bfs_follow=");
       usb_puti(bfs_follow_id);
       usb_puts("|");
       if (bfs_state == BFS_SEEK) {
         bfs_follow_id = bfs_follow();
         wl_do(); // update wireless
         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;
           bfs_check_id=0;
+        }
+      }
       if (bfs_state == BFS_FOLLOW) {
         /* check robot to follow */
         if (++bfs_check_id > BFS_CHECK_ID_TIME) {
           bfs_check_id=0;
           temp = bfs_follow();
           wl_do();
           if (temp == BFS_NO_VAL) {
             bfs_state = BFS_SEEK;
             return;
           } else if (temp != bfs_follow_id)
             bfs_follow_id = temp;
+        }
         // 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_SLOW_DISTANCE)
             bfs_speed = BFS_SLOW_SPEED;
           if (bfs_d2 < BFS_ORBIT_DISTANCE) {
             if (bfs_follow_id != bfs_otherRobot) {
               // check if we can now see a robot that's closer to our goal
               temp = bfs_follow();
               wl_do();
               if (temp == BFS_NO_VAL || temp == bfs_follow_id)
                 bfs_state = BFS_SEEK;
               else if (temp != bfs_follow_id)
                 bfs_follow_id = temp;
               return; // redo this function in light of the state changes
             } else {
               // orbit the robot
               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");
         orbit_state = ORBIT_INSERTION;
         while(orbit_state != ORBIT_STOP)
         { orbit_fsm(); }
         bfs_state = BFS_STOP; // move to stop state
+      }
       if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) {
         bfs_state = BFS_SEEK; // seek the otherRobot
         return;
+      }
       // 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_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;
+    }

     /** driver for orbit code
     	sit and activate bom, let other robot orbit this
     */
     #include <dragonfly_lib.h>
     #include <wireless.h>
     #include <wl_token_ring.h>
     int main(void) {
       // enable everything
       dragonfly_init(ALL_ON);
       orb_enable();
       orb_init();
       orb_set_color(PURPLE);
       wl_init();
       wl_set_channel(0xF);
       wl_token_ring_register();
       wl_token_ring_join(); // join token ring
       usb_init();
       usb_puts("start");
       usb_puti(wl_get_xbee_id());
       while(1) {
         wl_do(); /* do wireless */
+      }
       orb_set_color(RED);
       usb_puts("end");
       return 0;
+    }

     ########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 = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
     #/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

... This diff was truncated because it exceeds the maximum size that can be displayed.

Also available in: Unified diff