Colony

# 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

# 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

# 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

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

}

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include<encoders.h>

int master = 0;

int sending = 0;

int stop = 0;

void switch_sending ()

{

	if(sending)

	{

		sending = 0;

		bom_off();

	}

	else

	{

		sending = 1;

		bom_on();

	}

}

int main(void)

{

	int sending_counter = 0;

    /* Initialize dragonfly board */

    dragonfly_init(ALL_ON);

	xbee_init();

	wl_basic_init_default();

	wl_set_channel(12);

    int data_length;

	unsigned char *packet_data;

	char send_buffer;

	if(master) orb1_set_color(RED);

	else orb1_set_color(BLUE);

	while(1)

	{

		bom_refresh(BOM_ALL);

		if(master)

		{

			if(sending_counter++>100)

			{

				switch_sending();

				sending_counter = 0;

				send_buffer = 'a';

				wl_basic_send_global_packet(42, send_buffer, 1);

			}

			if(sending)

			{

			}

			else // recieving

			{

				if(bom_get_max()+1==5)

				{

				send_buffer = 's';

				wl_basic_send_global_packet(42, send_buffer, 1);

				}

			}

		}

		else // slave

		{

			if(packet_data[0]=='s') stop=1;

			if(packet_data[0]=='a') switch_sending();

			if(sending)

			{

				motor_l_set(FORWARD,0);

				motor_r_set(FORWARD,0);

			}

			else // recieving

			{

				if(stop)

				{

				}

				else

				{

				   if(bom_get_max()+1==8){	

					orb1_set_color(RED);

					motor_r_set(FORWARD,180);

					motor_l_set(FORWARD,180);

				   }

				   else {

					motor_l_set(FORWARD,0);

					motor_r_set(FORWARD,180);

					}

				}

			}

		}

		packet_data = wl_basic_do_default(&data_length);

		delay_ms(10);

	}

  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

# 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 <encoders.h>

#define RADIUS 300

/*

	This is a PREVIOUS VERSION of circle2.  Circle2 uses Echo's more exact code, while this uses a different method which is more buggy.

	This code has been kept for the time being, in case some of the code is useful for recylcling (the control loop, for instance) - without

	reverting the entire repositiory.

	This program is used to make robots target a center (leader) robot using the BOM,

	then drive toward it and stop at a certain distance away.

	The distance will eventually be adjustable.

	With adjustment, the leader robot will be able to turn and use its standardized

	rangefinder to reposition or space the robots evenly.

	AuTHORS: Niko, Alex

*/

extern void blink(int);

extern void search(void);

extern void approach(void);

extern void lastDrive(void);

int main(void)

{

	/* Initialize dragonfly board */

	dragonfly_init(ALL_ON);

	encoders_init();

	orb_set_color(GREEN);

	delay_ms(300);

	search();

	approach();

	while(1); /* END HERE */

	return 0;

}

void blink(int num)						// blinks the number of times specifed, with the orbs.

{										// useful for error checking.

	for(int i = 0; i<num; i++)

	{

		orb_set_color(ORB_OFF);

		delay_ms(200);

		orb_set_color(RED);

		delay_ms(200);

	}

	orb_set_color(ORB_OFF);

}

void search()	//makes the robot face the beacon

{

	orb_set_color(YELLOW);	//show that the robot is searching

	bom_refresh(BOM_ALL);

	int top=bom_get_max();

	if ((top<4) || (top>11))	//determine which way to turn

	{

		motor_r_set(BACKWARD,200);

		motor_l_set(FORWARD,200);

	}

	else if((top>4) && (top<12))

	{

		motor_r_set(FORWARD,200);

		motor_l_set(BACKWARD,200);

	}

	while (top != 4)

	{

		bom_refresh(BOM_ALL);

		top=bom_get_max();

	}

	motors_off();

}

void approach()					// drives to the right distance.

{

	int dist = 0;

	int temp = 0;

	int counter = 0;

//	int correct = 0;

	while(dist != RADIUS)		//puts the robots in the given distance RADIUS

	{

		temp=0;

		counter = 0;

		while(counter<8)		// averages the first 8 legitimate (non -1) readings

		{

			if(range_read_distance(IR2) == -1){}

			else

			{

				temp += range_read_distance(IR2);

				counter++;

			}

			delay_ms(20);

		}

		dist = temp/8;

		/*if (correct >2){

			orb_set_color(RED);

			break;

			}	// stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/

		bom_refresh(BOM_ALL);

		int top = bom_get_max();

		while(top!=4)

		{

			search();

			bom_refresh(BOM_ALL);

			top = bom_get_max();

		}	

		if ( dist<(RADIUS+30) && dist>(RADIUS-30)) {	// defines the dead zone +- 30 from the given radius

			orb_set_color(RED);

			break;

		}

		else if ((dist>RADIUS) || (dist==-1))

		{			

			///delay_ms(200);

			search();

			motor_r_set(FORWARD,200);

			motor_l_set(FORWARD,200);

		}

		else if ((dist<RADIUS))

		{

			//delay_ms(200);

			search();

			motor_r_set(BACKWARD,200);

			motor_l_set(BACKWARD,200);

		}

	}

	orb_set_color(BLUE);

	motors_off();

	/*usb_puti(range_read_distance(IR2));

	usb_puts("\n\r");

	motor_r_set(FORWARD,200);

	motor_l_set(FORWARD,200);

	delay_ms(300);

	usb_puti(range_read_distance(IR2));

	usb_puts("\n\r");

	delay_ms(300);

	usb_puti(range_read_distance(IR2));

	usb_puts("\n\r");

	delay_ms(300);

	usb_puti(range_read_distance(IR2));

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

}

void lastDrive()					// used for the final drive in lab1 and not useful here; control loop code may be recycled.

{

	int lspeed = 190;

	int rspeed = 190;

	int firstldist = encoder_get_x(LEFT);

	int firstrdist = encoder_get_x(RIGHT);

	int ldist = firstldist;

	int rdist = firstrdist;

	while ( (ldist-firstldist<800) && (rdist-firstrdist<800) )

	{

		motor_l_set(FORWARD,lspeed);

		motor_r_set(FORWARD,rspeed);

		int lvel = encoder_get_v(LEFT);

		int rvel = encoder_get_v(RIGHT);

		usb_puts("\n\rLeft distance: ");

		usb_puti(ldist-firstldist);

		usb_puts("\n\rRight distance: ");

		usb_puti(rdist-firstrdist);

		usb_puts("\n\rLeft velocity: ");

		usb_puti(lvel);

		usb_puts("\n\rRight velocity: ");

		usb_puti(rvel);

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

		if (rvel<lvel)					// adjust the speeds if the encoders have a mismatch.

		{

			rspeed += 1;

			lspeed -= 1;

		}

		else if (rvel>lvel)

		{

			rspeed -= 1;

			lspeed += 1;

		}

		ldist = encoder_get_x(LEFT);

		rdist = encoder_get_x(RIGHT);

		delay_ms(25);

	}

	motors_off();

	orb_set_color(GREEN);

}

# 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 <wl_basic.h>

#include <encoders.h>

#include "circle.h"

int EDGE = 0;

int BEACON = 1;

/*

	This program is used to make robots target a center (leader) robot using the BOM,

	then drive toward it and stop at a certain distance away.

	The distance will eventually be adjustable.

	With adjustment, the leader robot will be able to turn and use its standardized

	rangefinder to reposition or space the robots evenly.

	AuTHORS: Niko, Alex, Reva, Echo, Steve

*/

/* 

TODO:

	Check BOM more often and before final stop

	Make BOM check more comprehensively

	Use the center bot to check distances

		Count them ("spam" method)

		Use beacon to find relative positions

		Beacon tells them how to move to be at the right distance

	Wireless communication, initialization

*/

void forward(int speed){			// set the motors to this forward speed.

	motor_l_set(FORWARD,speed);

	motor_r_set(FORWARD,speed);

}

void left(int speed){				// turn left at this speed.

	motor_l_set(FORWARD,speed);

	motor_r_set(BACKWARD,speed);

}

void right(int speed){

	motor_l_set(BACKWARD,speed);

	motor_r_set(FORWARD,speed);

}

void stop(void){					// could be set to motors_off(), or just use this as an alternative.

	motor_l_set(BACKWARD,0);

	motor_r_set(FORWARD,0);

}

void setforward(int spd1, int spd2){

	motor_l_set(FORWARD,spd1);

	motor_r_set(FORWARD,spd2);

}

void backward(int speed){

	motor_l_set(BACKWARD, speed);

	motor_r_set(BACKWARD, speed);

}

int get_distance(void){				// takes an averaged reading of the front rangefinder

	int temp,distance,kk=5;			// kk sets this to 5 readings.

	distance =0;

	for (int i=0; i<kk; i++){

		temp = range_read_distance(IR2);

		if (temp == -1)

		{

			temp=0;

			kk--;

		}

		distance+= temp;

		delay_ms(3);

	}

	if (kk>0)

		return (int)(distance/kk);

	else 

		return 0;

}

void turn_to_beacon(int max){

	if (max>-1 && max<16){

		int index = (max+12)%16;

		if (index==0) { 

			stop(); 

		}

		else if (index<8) right(170);

		else left(170);

	}

}

void turn_to_beacon2(int max){				// like the previous but no stop() call

	if (max>-1 && max<16){

		int index = (max+12)%16;

		if (index==0) { 

		}

		else if (index<8) right(170);

		else left(170);

	}

}

void orient(void){

	int max_index = -1;

	while (max_index!=4) {

		/* Refresh and make sure the table is updated */

		bom_refresh(BOM_ALL);

		max_index = bom_get_max();

		turn_to_beacon(max_index);

		delay_ms(22);

	}

}

void orient2(void){

	int max_index = -1;

	while (max_index!=4) {

		/* Refresh and make sure the table is updated */

		bom_refresh(BOM_ALL);

		max_index = bom_get_max();

		turn_to_beacon2(max_index);

		delay_ms(22);

	}

}

void go_straight(void){						// drives forward a hardcoded distance. May not be useful.

	forward(200);

	encoder_rst_dx(LEFT);

	encoder_rst_dx(RIGHT);

	delay_ms(100); 

	int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);

	int count = 0;

	int d;

	while (count<25){						//count = 25 when bot6; count <12

		x_left = encoder_get_x(LEFT);

		x_right = encoder_get_x(RIGHT);

		d = x_right-x_left;

		if (d>13 || d<-13){

			if(d<50 && d>-50){

				d = round(1.0*d/4);

				setforward(200+d, 200-d);

			}

		}

		delay_ms(32);

		count++;

	}

}

void go_straight_onefoot(void){				// essentially, another name for the above.  Should be removed.

	go_straight();

}

void blink(int num) {

	for(int i = 0; i<num; i++)

	{

		orb_set_color(ORB_OFF);

		delay_ms(200);

		orb_set_color(RED);

		delay_ms(200);

	}

	orb_set_color(ORB_OFF);

}

int main(void)

{

	/* Initialize dragonfly board */

	dragonfly_init(ALL_ON);

	xbee_init();

	encoders_init();

	/* Initialize the basic wireless library */

	wl_basic_init_default();

	/* Set the XBee channel to your assigned channel */

	wl_set_channel(12);

	int robotid = get_robotid();

	char send_buffer[2];

	int data_length;

	unsigned char *packet_data=wl_basic_do_default(&data_length);

	int state = EDGE;

	if(wheel()<100)

	{

		state=EDGE;

	}

	else

	{

		state=BEACON;

	}

	int distance=1000;						// how far away to stop.

	int onefoot=300, speed=200;				// one foot is 490 for bot 1; one foot is 200 for bot6

	while(1) {

		switch(state) {

			case 0:			// EDGE

				orb_set_color(GREEN);

				while(1)

				{

					orb_set_color(YELLOW);

					packet_data=wl_basic_do_default(&data_length);

					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)

					{

						send_buffer[0]=CIRCLE_ACTION_ACK;

						send_buffer[1]=get_robotid();

						wl_basic_send_global_packet(42,send_buffer,2);

					}

					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)

					{

						break;

					}

				}

				orb_set_color(GREEN);

				orient();

				forward(speed);

				//range_init();

				orb_set_color(BLUE);

				distance = get_distance();

				while (distance>=onefoot || distance==0)

				{

					distance = get_distance();

					orient2();

					forward(speed);

					delay_ms(14);

				}

				stop();

				orient();

				//button1_wait ();			// code for lab1.

				//go_straight_onefoot();

				stop();

				break;

			case 1:			// BEACON

				bom_on();

				orb_set_color(PURPLE);

				int numrobots = 0;

				while(!button1_click())

				{ }

				send_buffer[0]=CIRCLE_ACTION_EXIST;

				send_buffer[1]=get_robotid();

				wl_basic_send_global_packet(42,send_buffer,2);

				packet_data=wl_basic_do_default(&data_length);

				while(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)

				{

					// IF NEEDED: a good place to collect and store the robot id.

					numrobots++;

				}

				send_buffer[0]=CIRCLE_ACTION_ACK;

				wl_basic_send_global_packet(42,send_buffer,2);

				blink(numrobots);

			break;

		}

	}

	orb_set_color(RED);

	while(1); /* END HERE */

	return 0;

}

#ifndef _CIRCLE_H

#define _CIRCLE_H

#include <inttypes.h>

/**** This file should not be edited! ****/

/*

 * The packet structure is 2 bytes

 * byte 0 is the action, which is one of the values below

 * byte 1 is the robot id

 */

#define CIRCLE_ACTION_EXIST 'E'

#define CIRCLE_ACTION_POSITION 'P'

#define CIRCLE_ACTION_ACK 'A'

//#define EDGE 0;

//#define BEACON 1;

#endif

# 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 <stdint.h>

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <stdlib.h>

int main (void) {

	Vector v;

	dragonfly_init(ALL_ON);

	xbee_init();

	encoders_init();

	orbs_set_color(BLUE, GREEN);

	delay_ms(1000);

	orbs_set_color(GREEN, BLUE);

	delay_ms(1000);

	orbs_set_color(RED, RED);

	while (1) {

		bom_print_usb(NULL);

		bom_get_vector(&v, NULL);

		usb_puts("x: ");

		usb_puti(v.x);

		usb_puts("\ty: ");

		usb_puti(v.y);

		usb_puts("\n");

		delay_ms(50);

	}

	while(1);

}

% vectorSizeSpecs - Script used to help calculate the max scalar value by

% 		which to multiply the BOM unit vectors (thus increasing the

%		precision of the scaled vectors) such that in the worst case, the

%		total net sum will not overflow the data type used to represent

%		the net vector components.

%

%	Author: John Sexton, Colony Project, CMU Robotics Club

% Parameters

maxIntensity = 255;

dataBits = 16;

scalar = 25;

n = 0:8;

angle = n * 2 * pi / 16;

vector_components = sin(angle);

max_net_component = sum(vector_components);

% Formula for max_net_component (mnc), obtained by summing

% values from the unit circle at angles of 0 : pi/8 : pi

% in quadrants I and II

mnc = 1;									%sin(pi/2)

mnc = mnc + (2*  (1/2)*sqrt(2+sqrt(2)));	%sin(3pi/8) and sin(5pi/8)

mnc = mnc + (2*  sqrt(2)/2);				%sin(pi/4) and sin(3pi/4)

mnc = mnc + (2*  (1/2)*sqrt(2-sqrt(2)));	%sin(pi/8) and sin(7pi/8)

calc_scalar = (2^(dataBits-1)-1) / (max_net_component * maxIntensity);

% calc_scalar = 25.560 for 16 bits => use 25 as scalar for int data type

fprintf('With %d data bits, calculated scalar value: %.3f\n\n', dataBits, calc_scalar);

% Check worst case

scaled_vector_components = scalar * vector_components;

worst_case = fix(scaled_vector_components) * maxIntensity;

worst_sum = sum(worst_case);

fprintf('With scalar %d, max worst case sum: %d\n', scalar, round(worst_sum));

fprintf('Max number: 2^(%d-1) - 1 = %d\n', dataBits, (2^(dataBits-1)-1));

% Calculate the x and y component arrays which should be used in the

% BOM Vector Component Tables in push_pull.c

N = 0:15;

x_comp = fix(scalar * cos(2 * pi / 16 * N))

y_comp = fix(scalar * sin(2 * pi / 16 * N))

# 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

# 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

/** 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;

  }

}

//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 175 // orbit distance as measured by rangefinders

#define ORBIT_STOP_DISTANCE 120

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

/** driver for bfs swarming

	execute bfs 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_set_channel(0xF);

  wl_token_ring_register();

  wl_token_ring_join(); // join token ring

  usb_init();

  bfs_init(6); // set robot_id to find

  usb_puts("start 4\n\r");

  while(1) {

    bfs_fsm(); // do bfs

  }

  return 0;

}

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