Colony

    wheels = inverseTransform * [v(i); omega(i)];

    wheels = min(wheels, wheelMax);

#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;

}

# this is a local makefile

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

#ifndef COLONYROOT

COLONYROOT = ..

# Target file name (without extension).

TARGET = main

# 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)

#else

COLONYROOT := ../$(COLONYROOT)

#endif

include $(COLONYROOT)/Makefile

#include <dragonfly_lib.h>

int main(void)

{

	/* initialize components, set wireless channel */

	dragonfly_init(ALL_ON);

  //Replace the next two lines with your own code

  //You can add as many lines as you want

  uint8_t rangeNumber[] = {IR1,IR2,IR3,IR4,IR5}; //0 indexed

  uint8_t cIndex;

  //initial setup (IR1)

  cIndex=0;

  orb1_set_color(BLUE);

  orb2_set_color(GREEN);

	while(1) {

		usb_puti(range_read_distance(rangeNumber[cIndex]));

		usb_putc('\r');

		if (button1_click()) {

			switch (cIndex){ //button1 chooses a color channel

				case 0: //IR1

					orb1_set_color(RED);

					orb2_set_color(RED);

					cIndex=1;

					break;

				case 1: //IR2

					orb1_set_color(GREEN);

					orb2_set_color(GREEN);

					cIndex=2;

					break;

				case 2: //IR3

					orb1_set_color(BLUE);

					orb2_set_color(BLUE);

					cIndex=3;

					break;

				case 3: //IR4

					orb1_set_color(YELLOW);

					orb2_set_color(YELLOW);

					cIndex=4;

					break;	

				case 4: //IR5

					orb1_set_color(BLUE);

					orb2_set_color(GREEN);

					cIndex=0;

					break;

				default: //undefined

					return 0;

					break;

			}

			delay_ms(50); //allows button press to release

		}

		delay_ms(50); //refresh frequency: 1000/(50ms) refresh rate

	}

  //this tell the robot to just chill out forever. don't put anything after this

  while(1);

	return 0;

}

# this is a local makefile

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

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = testIRcycle

# 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)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

# this is a local makefile

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

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = 

# 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)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

# this is a local makefile

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

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# 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)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

#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;

}

# this is a local makefile

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

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# 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)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile

// 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 170

#define BFS_SLOW_DISTANCE 250

#define BFS_SLOW_SPEED    160

#define BFS_ORBIT_DISTANCE 175 /* distance to start orbit around robot */

#define BFS_STOP_DISTANCE 120 /* distance to stop for object */

#define BFS_MAX_ROBOTS 20 /* max id of robot in project */

#define BFS_NO_VAL 255

#define BFS_CHECK_ID_TIME 100

int bfs_state;    /*State machine variable.*/

int bfs_speed;

int bfs_otherRobot; /* the robot we are seeking */

int bfs_my_id; /* my wireless id */

int bfs_follow_id; /* robot to follow */

int bfs_check_id; /* timer to check robot id to follow */

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

int bfs_d1,bfs_d2,bfs_d3,bfs_d4,bfs_d5;	/*The five distances taken in by IR.*/

int bfs_bom; /* bom data */

int bfs_bom_count;

#define BFS_MAX_BOM_COUNT 5 /* number of missing bom packets before reverting to seek state */

/* bfs_init

   argument: robot_id that you want to find

   notes: must call before bfs_fsm

*/

void bfs_init(int robot);

/* bfs_fsm

   argument: none

   notes: call in a while loop to perform FSM action

*/

void bfs_fsm(void);

#endif

#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_STOP_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/2,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;

  }

}