Colony

/** driver for lemmings code

	continually execute lemmings behavior

*/

#include <dragonfly_lib.h>

#include <wireless.h>

#include "lemmings.h"

/* ORB COLORS

  RED: light blue

  ORANGE: light blue

  YELLOW: light blue

  GREEN:

  BLUE:

*/

int main(void) {

  dragonfly_init(ALL_ON);

  wl_init();

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  lemmings_init();

  while(1) {

    wl_do();

    lemmings_FSM();

  }

  return 0;

}

#include <dragonfly_lib.h>

#include <wl_defs.h>

#include <wireless.h>

#include <wl_token_ring.h>

#include "smart_run_around_fsm.h"

#include "lemmings.h"

/*A simple behavior for following the leader.

  SINGLE_FILE pattern not implemented yet

*/

//The States:

#define LEAD 	1	// move around randomly, leading a chain

#define FOLLOW 	2	// follow another bot

#define FOLLOW_MULTI_DEFAULT 1 // set default multi-following pattern

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

/* Globals */

int cur_state; 		// current state

int crazy_count;    // Counter for a random behavior

int follow_bot;		// follow this bot (-1 if leader)

int follow_direction; // follow in this direction

int follow_multi;		// set to 0 for single-file following, 1 for tree following

/* Prototypes */

static void lemmings_set_pattern(int pattern);

static void lemmings_Prims(void);

static int lemmings_get_edge_weight(int robot1, int robot2);

static void lemmings_evaluate_state(void);

void lemmings_init() {

//  dragonfly_init(ALL_ON);

//  analog_init();

//  motors_init();

//  range_init();

//  orb_init();

//  orb_enable();

//  usb_init();

  run_around_init(); // prepare for moving

  /*Start in the default state, LEAD*/

  cur_state = LEAD;

  /*Set timers to their maximum values.*/

  crazy_count = CRAZY_MAX;

  // set to default state

  follow_bot = -1;

  cur_state = LEAD;

  usb_puts("init_following: robot #");

  usb_puti(follow_bot);

  // set pattern

  lemmings_set_pattern(FOLLOW_MULTI_DEFAULT);

}

// set the following pattern

// SINGLE_FILE if pattern <= 0

// TREE        if pattern >= 1

static void lemmings_set_pattern(int pattern) {

  if (pattern <= 0) {

    follow_multi = 0;

  } else {

    follow_multi = 1;

  }

  orb_set_color(ORANGE);

}

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

void lemmings_FSM(void) {

  int bom = -1;

  int i = 0;

  int old_follow_bot;

  orb_set_color(WHITE);

  if (follow_bot < 0) {

    lemmings_Prims(); // if we are not following, then attempt to follow

  }

  // if we are following a bot, make sure we can still see it

  old_follow_bot = follow_bot;

  for (i=0;follow_bot>=0&&i<10;i++) {

    bom = wl_token_get_sensor_reading(wl_get_xbee_id(),follow_bot);

    if (bom < 0) {

      // get new bot to follow

      lemmings_Prims();

      if (old_follow_bot == follow_bot)

        break; // some sort of error

      continue;

    } else {

      cur_state = FOLLOW; // set to follow state

      switch(bom) {

      case 0:

      case 15:

      case 14:

      case 13:

      case 12:

        follow_direction = 255;

        break;

      case 1:

        follow_direction = 150;

        break;

      case 2:

        follow_direction = 100;

        break;

      case 3:

        follow_direction = 50;

        break;

      case 4:

        follow_direction = 0;

        break;

      case 5:

        follow_direction = -50;

        break;

      case 6:

        follow_direction = -100;

        break;

      case 7:

        follow_direction = -150;

        break;

      case 8:

      case 9:

      case 10:

      case 11:

        follow_direction = -255;

        break;

      default:

        follow_direction = 0;

        break;

      }

      break; // break from loop

    }

  }

  /*If the crazy count is in it's >>3 range, and we are currently a follower, be a leader.*/

  /* CURRENTLY DISABLED

  if(cur_state == FOLLOW && crazy_count<=(CRAZY_MAX>>3)) {

    cur_state = LEAD;

  }*/

  usb_puts("following: ");

  usb_puti(follow_bot);

  usb_puts("bom: ");

  usb_puti(bom);

  usb_puts("\n\r");

  lemmings_evaluate_state(); // evaluate state

}

// create connected components and pick a leader for each chain

// use modified Prim's alogrithm to find local spanning tree

// CURRENTLY JUST LOOKS FOR CLOSEST ROBOT TO FOLLOW

static void lemmings_Prims(void) {

  int nodeA = wl_get_xbee_id(); // get id of current robot

  int nodeB = -1;

  int cur_weight = -1;

  int best_weight = 1000; // set to infinity initially

  int best_node = -1;

  int i = 0;

  orb_set_color(BLUE);

  // iterate through token ring

  wl_token_iterator_begin();

  for(i=0; wl_token_iterator_has_next() && i<10; i++) {

    nodeB = wl_token_iterator_next();

    usb_puts("botB: ");

    usb_puti(nodeB);

    usb_puts("\n\r");

    if (nodeB < 0) {

      break; // no more bots

    } else if (nodeB == nodeA) {

      continue; // can't follow self

    }

    cur_weight = lemmings_get_edge_weight(nodeA,nodeB);

    usb_puts("weight: ");

    usb_puti(cur_weight);

    usb_puts("\n\r");

    if (cur_weight >= 0 && cur_weight < best_weight) {

      // this is new best node, so save values

      best_weight = cur_weight;

      best_node = nodeB;

    }

  }

  if (best_node == -1 || (best_weight > 5 && nodeA < nodeB)) {

    // no robots to follow, or choosing to be a leader

    cur_state = LEAD;

    follow_bot = -1;

  } else {

    // follow bot

    cur_state = FOLLOW;

    follow_bot = nodeB;

  }

}

// get edge weight of sensor matrix

// add in BOM range data when BOM 1.5 comes out

static int lemmings_get_edge_weight(int robot1, int robot2) {

  int weight = -1;

  int bom = wl_token_get_sensor_reading(robot1,robot2);

  // since we only have positioning data, give better weight to

  // bots in from of us

  switch(bom) {

  case 12:

    weight++;

  case 13:

  case 11:

    weight++;

  case 14:

  case 10:

    weight++;

  case 15:

  case 9:

    weight++;

  case 0:

  case 8:

    weight++;

  case 1:

  case 7:

    weight++;

  case 2:

  case 6:

    weight++;

  case 3:

  case 5:

    weight++;

  case 4:

    break;

  default:

    weight = -1;

    break;

  }

  return weight;

}

//Acts on state change.

static void lemmings_evaluate_state() {

  usb_puts("state: ");

  usb_puti(cur_state);

  usb_puts("\n\r");

  switch(cur_state){

  case(LEAD):

    //move_avoid(200,0,50);

    //orb_set_color(GREEN);

    run_around_FSM(); // do random run around

    break;

  case(FOLLOW):

    move(200,follow_direction); // move in direction of bot

    orb_set_color(RED);

    break;

  default:

    /*Should never get here, go strait.*/

	  move(200,0);

	  orb_set_color(BLUE);

	  break;

	}

}

/**

 */

#ifndef _RUN_AROUND_FSM_H_

#define _RUN_AROUND_FSM_H_

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 = /dev/tty.usbserial-A4001hyO

#

#

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

# 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

/**

 */

#ifndef _LEMMINGS_H_

#define _LEMMINGS_H_

void lemmings_init(void);

void lemmings_FSM(void);

#endif

/**

 */

#include "dragonfly_lib.h"

#include "smart_run_around_fsm.h"

/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.

Could be better at not getting stuck.

Latest revision only has two accessible states: move and reverse.

*/

//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 185    //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.*/

static void evaluate_state(void);

void run_around_init(void)

{

  range_init();

  analog_init();

  motors_init();

  orb_init();

  orb_enable();

  usb_init();

  /*Start in the default state, MOVING*/ 

  avoid_state=MOVING;

  /*Set timers to their maximum values.*/

  crazy_count=CRAZY_MAX;

  backup_count=0; 

  pControl=0;

  /*Initialize distances to zero.*/ 

  d1=0; d2=0; d3=0; d4=0; d5=0;

  orb_set_color(GREEN);

}

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

void run_around_FSM(void) {

  /*Default to moving.*/ 

  avoid_state=MOVING;

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

  will not update the distances.*/ 

  int temp;

  temp=range_read_distance(IR1);

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

  temp=range_read_distance(IR2);

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

  temp=range_read_distance(IR3);

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

  temp=range_read_distance(IR4);

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

  temp=range_read_distance(IR5);

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

  /*If the crazy count is in it's >>3 range, it acts crazy.*/

  if(crazy_count<=(CRAZY_MAX>>3))

  {

    avoid_state=CRAZY;

    crazy_count--;

    if(crazy_count<0) crazy_count=CRAZY_MAX;

    evaluate_state();

    return;

  }

  //Checks the forward distance to see if it should back up, if so...state backwards.

  if((d2!=-1)&&(d2 < 150)){

      backup_count=BACKUP_MAX;

      avoid_state=BACKWARDS;

      evaluate_state();

      return;

  }

  /*

  if(d1 < 120 || d3 < 120) {

		avoid_state = BACKWARDS;

		backup_count = BACKUP_MAX;

		evaluate_state();

		return;

  }

  */

  if(backup_count<BACKUP_MAX){

    avoid_state=BACKWARDS; 

    if(backup_count<0)

      backup_count=BACKUP_MAX;

    evaluate_state();

    return;

  }

  /*Should evaluate an expression from -255 to 255 to pass to move.*/

  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;

  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;

  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/

  /*Debug stuff:*/

  /*usb_puts("pControl evaluating: ");

  usb_puti(pControl);

  usb_puts("\n\r");

  usb_puts("IR1: ");

  usb_puti(d1);

  usb_puts(" IR2: ");

  usb_puti(d2);

  usb_puts(" IR3: ");

  usb_puti(d3);

  usb_puts(" IR4: ");

  usb_puti(d4);

  usb_puts(" IR5: ");

  usb_puti(d5);

  usb_puts("\n\r");*/

  evaluate_state();

}

//Acts on state change.

static void evaluate_state(){

    switch(avoid_state){

    case(MOVING): orb_set_color(GREEN);

      move(STRAIT_SPEED,-pControl);

      break;

    case(BACKWARDS): orb_set_color(ORANGE);

      move(-STRAIT_SPEED,0);

      break;

    case(CRAZY): orb_set_color(RED);

      /*TODO: Implement a crazy state.*/

      move(STRAIT_SPEED,-pControl);

      break;

    default:

      /*Should never get here, go strait.*/

      move(100,0); orb_set_color(BLUE);

      break;

  }

}