Colony

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

#include "circle.h"

int EDGE = 0;

int BEACON = 1;

int timeout = 0;

int sending = 0;

int stop2 = 0;

struct vector slave_position;

int desired_max_bom;

int bom_max_counter;

//Last used 12,13,7(BOM)

void set_desired_max_bom(int desired_angle)

{

	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;

	if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;

	if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;

	if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;

	if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;

	if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;

	if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;

	if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;

	if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;

	if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;

	if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;

	if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;

	if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;

	if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;

	if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;

	if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;

}

void switch_sending(void)

{

	if(sending)

	{

		sending = 0;

		bom_off();

	}

	else

	{

		sending = 1;

		bom_on();

	}

}

/*

Current situation:

	go to the center. ends when each edge robto send "I am here" message

I think compasses are in the club, but not on the robots?

Assume we have the compasses, and write pseudocode, maybe?

TODO:

  Center:

    1. Make a move forward method/state to make the circle begin moving as a group.

  Edge: 

    2. Find a way to follow the middle robot and preserve circle structure, or follow a directional command given by the center robot.

    3. Fix the error case with wireless - if the center robot does not receive the first exist packet, the robot keeps sending it and will cause error later.

  In general:

    More testing.  Especially with robots that are actually working.

*/

/*

	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: Nico, Alex, Reva, Echo, Steve

*/

/* 

TODO:

	Used: Bots 1, 7

	16 Performed Badly

	12 worked ok as beacon, not well as edge

		Fix orient code so the bot does not toggle back and forth when it tries to turn

		Use the center bot to check distances

Done-->	Count them ("spam" method)

		Use beacon to find relative positions

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

		*done*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);			// stop() is better - motors_off() creates a slight delay to turn them back on.

	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;

			i--;

		}

		else

			distance+= temp;

		delay_ms(3);

	}

	if (kk>0)

		return (int)(distance/kk);

	else 

		return 0;

}

/*

	Orients the robot so that it is facing the beacon (or the broadcasting BOM).

*/

void correctTurn(void)

{

	orb1_set_color(BLUE);			// BLUE and PURPLE

	left(220);

	while(1)

	{

		int bomNum = 0;				// bomNum is the current maximum reading

		bom_refresh(BOM_ALL);

		bomNum = bom_get_max();

		usb_puti(bomNum);

		if(bomNum == 4)				// when it's turned the right way, stop

		{

			timeout = 0;

			//motor_l_set(1, 200);

			//motor_r_set(1, 200);

			break;				// exits the while() loop to stop the method

		}

		else					// facing the wrong way

		{

			if(bomNum == -1)

			{

				timeout++;

				if(timeout > 5000)	// if it's been looking too long, move a little bit as it turns

				{

					motor_r_set(FORWARD, 210);

					motor_l_set(BACKWARD, 190);

				}

			}

			else if((bomNum >= 12) || (bomNum < 4))

			{

				motor_l_set(FORWARD, 200);

				motor_r_set(BACKWARD, 200);

				timeout = 0;

			}

			else

			{

				motor_l_set(BACKWARD, 200);

				motor_r_set(FORWARD, 200);

				timeout = 0;

			}

		}

	}

	return;

}

/*

	This is supposed to (essentially) do a correct turn, then move forward or something.

	Actually, we should just get rid of this.

*/

void correctApproach(void)

{

	int bomNum = 0;

	bom_refresh(BOM_ALL);

	bomNum = bom_get_max();

	usb_puti(bomNum);

	if(bomNum == 4)

	{	

		motor_l_set(1, 200);

		motor_r_set(1, 200);

	}

	else

	{

		if(bomNum == -1){}

		else if((bomNum >= 12) || (bomNum < 4))

		{

			motor_l_set(FORWARD, 200);

			motor_r_set(BACKWARD, 200);

		}

		else

		{

			motor_l_set(BACKWARD, 200);

			motor_r_set(FORWARD, 200);

		}

		delay_ms(100);

	}

}

/* 

	drives forward a hardcoded distance. May not be useful.

*/

void go_straight(void){

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

	}

}

/* 

    BLINK the given number times

*/

void blink(int num) {

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

	{

		orb_set_color(ORB_OFF);

		delay_ms(350);

		orb_set_color(RED);

		delay_ms(200);

	}

	orb_set_color(ORB_OFF);

}

//*****************************************************************************************************************************************************************************************

/*

	A double state machine.  The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading.

*/

int main(void)

{

	/* Initialize dragonfly board */

    	dragonfly_init(ALL_ON);

    	/* Initialize the basic wireless library */

    	wl_basic_init_default();

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

	wl_set_channel(24);

	int sending_counter = 0;

	int robotid = get_robotid();

	int used[16];

	int usedPointer=0;

	for (int i=0; i<16; i++) used[i] = 0;

	char send_buffer[2];

	int data_length;

	unsigned char *packet_data=wl_basic_do_default(&data_length);

	int state = EDGE;

	int beacon_State=0;

	int edge_State=0;

	int waitingCounter=0;

	int robotsReceived=0;

	int offset = 20, time=0;

	if(wheel()<100)

	{

		state=EDGE;

	}

	else

	{

		state=BEACON;

	}

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

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

	while(1)

	{

		bom_refresh(BOM_ALL);

		/*

		*******TERMinology**************

		EDGE=0 other names: slave. Definition: robots on the edge of  the circle;

		BEACON=1 other name: master. Definition: robots in the center of the circle;

		*******EXPECTED MOVES ********** 

		The designed movement:

		 1. one center robot, several edge robots are on;

		 2. center robots: button 1 is pressed;

		 3. center robots: send global package telling edges that he exists;

		 4. EDGE robots response with ACK. 

		 5. EDGE robots wait for center robots to finish counting (DONE package)

		 6. EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center

		*/

		// decide if its is center or not.

		switch(state)

		{

			/**********

				if  EDGE /slave robots

			*/

			case 0:	

				switch(edge_State)

				{

					/*

						0. EDGE robots are on. 

						1. They are waiting for ExiST pacakage from the Center robots

						2. After they receive the package, they send ACK package to center.

						3. Wait the center robot to finish counting all EDGE robots

					*/

					case 0:   

						bom_off();

						orb1_set_color(YELLOW);orb2_set_color(CYAN);

						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]=robotid;

							wl_basic_send_global_packet(42,send_buffer,2);

							edge_State=1;

						}

					break;

					/*

						1. Wait for DONE package 

						2. The counting process is DONE

					*/

					case 1:		

						orb_set_color(YELLOW);orb2_set_color(PURPLE);

						packet_data=wl_basic_do_default(&data_length);

						//wl_basic_send_global_packet(42,send_buffer,2);

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

						{

							edge_State=2;

						}

					break;

					case 2:

						// COLOR afer DONE ---> MAGENTA

						orb_set_color(MAGENTA);

						correctTurn();			// turn to face the beacon

						forward(175);

						//range_init();

						distance = get_distance();

						time=0;

						while ((distance-offset)>=onefoot || distance==0 || (distance+offset)<onefoot)

						{

							if(distance==0)

								orb_set_color(WHITE);

							else if(distance-offset>=onefoot)

								forward(175);

							else

								backward(175);

							//correctApproach();

							distance = get_distance();

							delay_ms(14);

							time+=14;

							if(time>500){

								correctTurn();

								time=0;

							}

						}

						stop();

						orb_set_color(LIME);

						send_buffer[0]=CIRCLE_ACTION_ACK;

						send_buffer[1]=robotid;	

						wl_basic_send_global_packet(42,send_buffer,2);

						edge_State=3;

					break;

					// wait until the beacon sends out its robot ID

/*

	// as we don't check distance to center one by one now,  

	no more private communication at this point. 

					case 3:

						orb_set_color(YELLOW);

						orb2_set_color(GREEN);

						packet_data=wl_basic_do_default(&data_length);

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

						{

							bom_on();

							orb_set_color(ORANGE);

						}

						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1]==robotid)

						{

							bom_off();

							orb_set_color(YELLOW);

						}

					break;

*/

				}

*/				

				// NEW CODE FROM JOEL

				/*

				while(1)

				{

					packet_data = wl_basic_do_default(&data_length);

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

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

					if(sending)

					{

					}

					else // recieving

					{

						if(stop2)

						{

							motor_l_set(FORWARD,0);

							motor_r_set(FORWARD,0);

							orb1_set_color(GREEN);

						}

						else

						{

							int max_bom = bom_get_max();

								/*usb_puts("bom_get_max : ");

							usb_puti(max_bom);

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

					/*	

							if(max_bom == 8)

							{	

								orb2_set_color(BLUE);

								motor_r_set(FORWARD,180);

								motor_l_set(FORWARD,180);

							}

							else if(max_bom == 7 || max_bom == 6 || max_bom == 5)

							{

								motor_l_set(FORWARD,180);

								motor_r_set(FORWARD,0);

							}

							else if(max_bom == -1);

							else 

							{

								orb2_set_color(GREEN);

								motor_l_set(FORWARD,0);

								motor_r_set(FORWARD,180);

							}

						}

					}

				delay_ms(10);

				} //end while

				*/

				//break;

				// END for EDGE robots

//*****************************************************************************************************************************************************************************************

			/*

			   The CENTER/BEACON/MASTER robot

			*/	

			case 1:			// BEACON /master /enter robots

				switch(beacon_State) {

				/*

				   0. center  robots on wait for pressing button 1

				*/

				case 0:		

					bom_on();

					orb_set_color(PURPLE);

					if(button1_click()) beacon_State=1;

					break;	

				/*

					1. Send EXIST package to EDGE robots

				*/

				case 1:		// sends a global exist packet to see how many robots there are

					orb_set_color(RED);

					send_buffer[0]=CIRCLE_ACTION_EXIST;

					send_buffer[1]=get_robotid();

					wl_basic_send_global_packet(42,send_buffer,2);

					usedPointer = 0;

					beacon_State=2;

					// wait for the edge to send for 1sec. 

					delay_ms(1000);

					break;

				/*

					2. Count the number of the EDGE robots 

					*******NOTE: at most  1500  times of loop ******

				*/

				case 2: 	

					waitingCounter++;

					orb1_set_color(YELLOW);

					orb2_set_color(BLUE);

					packet_data=wl_basic_do_default(&data_length);

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

					{

						orb_set_color(RED);

						orb2_set_color(BLUE);

						//only add to robots seen if you haven't gotten an ACK from this robot

						if(used[packet_data[1]]==0){		

							robotsReceived++;

							used[usedPointer] = packet_data[1];

							usedPointer++;

							usb_puts("Added: ");

							usb_puti(packet_data[1]);

							usb_puts("\r\n");

						}

					}

					if(waitingCounter >= 2000){

						beacon_State=3;

					}

					break;

				/*

					COUNTing is DONE.

					Sending DONE package.

				*/	

				case 3:

					blink(robotsReceived);

					orb_set_color(GREEN);

					send_buffer[0]=CIRCLE_ACTION_DONE;

					wl_basic_send_global_packet(42,send_buffer,2);

					beacon_State=4;

					break;

				/*

					Wait for all the robots to get to right distance/position 

				*/

				case 4: 

					orb1_set_color(YELLOW);

					orb2_set_color(WHITE);

					int numOk = 0;

					while(numOk<robotsReceived)

					{

						packet_data=wl_basic_do_default(&data_length);

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

						{

							numOk++;

						}

					}

					beacon_State = 5;

					break; 

				/**

 TODO: no need for "private" communication if we don't check distance one by one  

   So comment out this part at least for now.

				    Starts repositioning the edge robots one by one

				*/

/*				case 5:

					orb_set_color(MAGENTA);

					bom_off();

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

					{

						usb_puts("used = ");

						usb_puti(used[i]);

						usb_puts("\r\n");

						send_buffer[0]=CIRCLE_ACTION_ACK;

						send_buffer[1]=used[i];

						wl_basic_send_global_packet(42,send_buffer,2);

						delay_ms(1000);

						send_buffer[0]=CIRCLE_ACTION_DONE;

						send_buffer[1]=used[i];

						wl_basic_send_global_packet(42,send_buffer,2);

						delay_ms(1000);

					}

*/

					/*orb_set_color(BLUE);

					// clock for switching the BOMs between the master and slave

					if(sending_counter++>4)	

					{

						switch_sending();

						sending_counter = 0;

						send_buffer[0] = 'a';

						wl_basic_send_global_packet(42, send_buffer, 1);

					}

					if(sending)

					{

					}

					else // recieving

					{

						int max_bom = bom_get_max();

						usb_puts("bom_get_max : ");

						usb_puti(max_bom);

						usb_puts("\n\r");

						if(max_bom == desired_max_bom)

						{

							// only stops the slave if two consecutive boms 

							//     reading give the desired bom as the max one. Filters the noise.

							if(bom_max_counter)                 

							{

								send_buffer[0] = 's';

								wl_basic_send_global_packet(42, send_buffer, 2);

							}

							bom_max_counter =1;

						}

						else bom_max_counter = 0;

					}

					*/

					//break;

			}

		}

	}

	orb_set_color(RED);

	while(1); /* END HERE */

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

#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

#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 CIRCLE_ACTION_DONE 'D'

//#define EDGE 0;

//#define BEACON 1;

#endif

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include<encoders.h>

int master = 0;

int sending = 0;

int stop = 0;

struct vector slave_position;

int desired_max_bom;

int bom_max_counter;

void set_desired_max_bom(int desired_angle)

{

	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;

	if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;

	if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;

	if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;

	if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;

	if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;

	if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;

	if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;

	if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;

	if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;

	if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;

	if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;

	if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;

	if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;

	if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;

	if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;

}

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[2];

	if(master) {orb1_set_color(RED); set_desired_max_bom(30);}

	else orb1_set_color(BLUE);

	while(1)

	{

		bom_refresh(BOM_ALL);

		if(master)

		{

			if(sending_counter++>4)	// clock for switching the BOMs between the master and slave

			{

				switch_sending();

				sending_counter = 0;

				send_buffer[0] = 'a';

				wl_basic_send_global_packet(42, send_buffer, 1);

			}

			if(sending)

			{

			}

			else // recieving

			{

				int max_bom = bom_get_max();

					usb_puts("bom_get_max : ");

					usb_puti(max_bom);

					usb_puts("\n\r");

				if(max_bom == desired_max_bom)

				{

					if(bom_max_counter)                 // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise.

					{

					send_buffer[0] = 's';

					wl_basic_send_global_packet(42, send_buffer, 2);

					}

				bom_max_counter =1;

				}

				else bom_max_counter = 0;

			}

		}

		else // slave

		{

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

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

			if(sending)

			{

			}

			else // recieving

			{

				if(stop)

				{

				motor_l_set(FORWARD,0);

				motor_r_set(FORWARD,0);

				orb1_set_color(GREEN);

				}

				else

				{

					int max_bom = bom_get_max();

					/*usb_puts("bom_get_max : ");

					usb_puti(max_bom);

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

				   if(max_bom == 8)

				   {	

					orb2_set_color(BLUE);

					motor_r_set(FORWARD,180);

					motor_l_set(FORWARD,180);

				   }

				   else if(max_bom == 7 || max_bom == 6 || max_bom == 5)

				   {

					motor_l_set(FORWARD,180);

					motor_r_set(FORWARD,0);

					}

					else if(max_bom == -1);

				   else 

				   {

					orb2_set_color(GREEN);

					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 'COM7:'; 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 <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

/*

 * Hunter-Prey main.c File - Implementation of Hunter-Prey behavior which

 * 		uses finite state machines to manage the behavior. A top level

 * 		state machine controls the high level behavior switches between

 * 		"hunter" and "prey" and manages the wireless communication. Two

 * 		additional state machines control the behavior of the robot when

 * 		it is in "prey" mode and when it is in "hunter" mode.

 *

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

 */

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include "hunter_prey.h"

#include "encoders.h"

#define WL_CHANNEL 					24

#define BACK_THRESHOLD 			-1000

#define TURN_DIST						1024

#define IR_DIST_THRESHOLD		150

#define WAIT_DELAY_MS				2000

/* State Macros */

/* Top Level FSM States */

#define TOP_INIT						0

#define TOP_HUNTER_HUNT			1

#define TOP_HUNTER_TAG			2

#define TOP_HUNTER_PURSUE		3

#define TOP_PREY_AVOID			4

#define TOP_HUNTER_WAIT			5

#define TOP_ERROR						6

/* Hunter FSM States */

#define HUNTER_SPIRAL				0

#define HUNTER_CHASE				1

/* Prey FSM States */

#define PREY_START_BACK			0

#define PREY_BACKING				1

#define PREY_TURN						2

#define PREY_AVOID					3

/* Function prototype declarations */

int hunter_FSM(int, int, int);

int prey_FSM(int);

/* Variables used to receive packets */

unsigned char* packet_data;

int data_length;

/*  Data buffer used to send packets */

char send_buffer[2];

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 assigned channel */

    wl_set_channel(WL_CHANNEL);

    /* ****** CODE HERE ******* */

		/* Initialize state machines */

		int state = TOP_INIT;

		int hunter_state = HUNTER_SPIRAL;

		int prey_state = PREY_AVOID;

		int frontIR = 0;

		int maxBOM = 0;

		int robotID = get_robotid();

		int oldTime = 0, curTime = 0;

		while (1) {

			/* Check if we've received a wireless packet */

			packet_data = wl_basic_do_default(&data_length);

			/* Top level state machines */

			switch(state) {

				case TOP_INIT:

					orbs_set_color(RED, GREEN);

					delay_ms(500);

					orbs_set_color(GREEN, RED);

					delay_ms(500);

					/* Allow user to pick the starting behavior */

					if (button1_read()) {

						state = TOP_PREY_AVOID;

						prey_state = PREY_AVOID;

					} else {

						state = TOP_HUNTER_HUNT;

						hunter_state = HUNTER_SPIRAL;

					}

					break;

				case TOP_HUNTER_HUNT:

					orbs_set_color(RED, RED);

					if (packet_data && data_length == 2

							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {

						/* If we've received an ACK, we need to wait */

						state = TOP_HUNTER_WAIT;

					} else {

						/* Record some sensor readings and check if we can TAG */

						bom_refresh(BOM_ALL);

						frontIR = range_read_distance(IR2);

						maxBOM = get_max_bom();

						if (hunter_prey_tagged(maxBOM, frontIR)) {

							state = TOP_HUNTER_TAG;

						} else {

							/* If we haven't tagged, then enter hunter FSM */

							hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);

						}

					}

					break;

				case TOP_HUNTER_TAG:

					orbs_set_color(RED, PURPLE);

					if (packet_data && data_length == 2

							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {

						/* If we've received an ACK, then someone beat us to the TAG and

 						 * we need to wait. */

						state = TOP_HUNTER_WAIT;

					} else {

						/* Prepare and send the TAG packet */

						send_buffer[0] = HUNTER_PREY_ACTION_TAG;

						send_buffer[1] = robotID;

						wl_basic_send_global_packet(42, send_buffer, 2);

						/* Record the time so we don't spam a TAG message on the network */

						oldTime = rtc_get();

						state = TOP_HUNTER_PURSUE;

					}

					break;

				case TOP_HUNTER_PURSUE:

					orbs_set_color(RED, BLUE);

					curTime = rtc_get();

					if (packet_data && data_length == 2

							&& packet_data[0] == HUNTER_PREY_ACTION_ACK) {

						/* Check if we've received a new wireless packet */

						if (packet_data[1] == robotID) {

							/* We've been ACKed, so we can now become the prey */

							state = TOP_PREY_AVOID;

							prey_state = PREY_START_BACK;

						} else {

							/* If we get an ACK with a different robotID, then someone beat us

							 * to the TAG, so we must wait */

							state = TOP_HUNTER_WAIT;

						}

					} else if (curTime - oldTime > 1) {

						/* If 1 second has ellapsed, return to normal hunting state (we can

						 * TAG again now) */

						state = TOP_HUNTER_HUNT;

					} else if (oldTime > curTime) {

						/* If for some reason the timer overflows, or the wireless library

						 * (which is also using the same timer) resets the timer,

						 * reinitialize the timer so that we don't wait too long for the

						 * timer to catch back up. */

						oldTime = curTime;

					} else {

						/* If no other behavioral changes need to be made, then continue

						 * with the hunter FSM where we left off */

						bom_refresh(BOM_ALL);

						frontIR = range_read_distance(IR2);

						maxBOM = get_max_bom();

						hunter_state = hunter_FSM(hunter_state, maxBOM, frontIR);

					}

					break;

				case TOP_PREY_AVOID:

					orbs_set_color(GREEN, GREEN);

					if (packet_data && data_length == 2

							&& packet_data[0] == HUNTER_PREY_ACTION_TAG) {

						/* Check if we've received a TAG yet. If so then send an ACK back */

						send_buffer[0] = HUNTER_PREY_ACTION_ACK;

						send_buffer[1] = packet_data[1];

						wl_basic_send_global_packet(42, send_buffer, 2);

						state = TOP_HUNTER_WAIT;

					} else {

						/* If we haven't received a TAG yet, continue with prey FSM */

						bom_on();

						prey_state = prey_FSM(prey_state);

					}					

					break;

				case TOP_HUNTER_WAIT:

					/* Set orb colors and wait to give the prey the 5 second head start */

					orbs_set_color(BLUE, BLUE);

					bom_off();

					motors_off();

					delay_ms(WAIT_DELAY_MS);

					state = TOP_HUNTER_HUNT;

					hunter_state = HUNTER_SPIRAL;

					break;

				case TOP_ERROR:

				default:

					orbs_set_color(PURPLE, PURPLE);

					state = TOP_ERROR;

					while(1);

					break;

			}

		}

    /* ****** END HERE ******* */

    while(1);

    return 0;

}

/*

 * prey_FSM - Prey finite state machine which starts by backing away, turning,

 * 		and then running and avoiding obstacles.

 *

 * Arguments:

 * 	prey_state - Current prey state.

 *

 * returns - The new state of the prey state machine.

 */

int prey_FSM(int prey_state) {

	/* Variable to store the front rangefinder readings */

	int rangeVals[3] = {0, 0, 0};

	switch (prey_state) {

		case PREY_START_BACK:

			motor_l_set(BACKWARD, 255);

			motor_r_set(BACKWARD, 255);

			encoder_rst_dx(LEFT);

			encoder_rst_dx(RIGHT);

			return PREY_BACKING;

			break;

		case PREY_BACKING:

			if (encoder_get_x(LEFT) < BACK_THRESHOLD

											|| encoder_get_x(RIGHT) < BACK_THRESHOLD) {

				motor_l_set(BACKWARD, 255);

				motor_r_set(FORWARD, 255);

				encoder_rst_dx(LEFT);

				encoder_rst_dx(RIGHT);

				return PREY_TURN;

			} else {

				return PREY_BACKING;

			}

			break;

		case PREY_TURN:

			if (encoder_get_x(LEFT) < -TURN_DIST

											|| encoder_get_x(RIGHT) > TURN_DIST) {

				return PREY_AVOID;				

			} else {

				return PREY_TURN;

			}

			break;

		case PREY_AVOID:

			rangeVals[0] = range_read_distance(IR1);

			rangeVals[1] = range_read_distance(IR2);

			rangeVals[2] = range_read_distance(IR3);

			/* Drive away if we detect obstacles using the rangefinders */

			if (rangeVals[1] > 0 && rangeVals[1] < IR_DIST_THRESHOLD) {

				if (rangeVals[0] < rangeVals[2]) {

					motor_l_set(FORWARD, 255);

					motor_r_set(BACKWARD, 255);

				} else {

					motor_l_set(BACKWARD, 255);

					motor_r_set(FORWARD, 255);

				}

				return PREY_AVOID;

			} else if (rangeVals[0] > 0 && rangeVals[0] < IR_DIST_THRESHOLD) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 170);

				return PREY_AVOID;

			} else if (rangeVals[2] > 0 && rangeVals[2] < IR_DIST_THRESHOLD) {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 255);

				return PREY_AVOID;

			} else {			

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 255);

				return PREY_AVOID;

			}

			break;

		default:

			return PREY_AVOID;

			break;

	}

	return prey_state;

}

/*

 * hunter_FSM - Hunter finite state machine which defaults to spiraling

 * 		outwards until the BOM can locate the prey. Once the BOM locates

 * 		the prey, chase the prey as fast as possible.

 *

 * Arguments:

 * 	hunter_state - Current hunter state.

 * 	maxBOM - Current maximum BOM value.

 * 	frontIR - Current front IR rangefinder reading value.

 *

 * returns - The new state of the hunter state machine.

 */

int hunter_FSM(int hunter_state, int maxBOM, int frontIR) {

	switch(hunter_state) {

		case HUNTER_SPIRAL:

			if (maxBOM != -1) {

				return HUNTER_CHASE;

			} else {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 190);

				return HUNTER_SPIRAL;

			}

			break;

		case HUNTER_CHASE:

			if (maxBOM == -1) {

				return HUNTER_CHASE;

			} else if (maxBOM == 4) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM == 3) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 240);

				return HUNTER_CHASE;

			} else if (maxBOM == 5) {

				motor_l_set(FORWARD, 240);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM < 3) {

				motor_l_set(FORWARD, 255);

				motor_r_set(FORWARD, 170);

				return HUNTER_CHASE;

			} else if (maxBOM > 5 && maxBOM <= 8) {

				motor_l_set(FORWARD, 170);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else if (maxBOM > 8 && maxBOM < 12) {

				motor_l_set(BACKWARD, 255);

				motor_r_set(FORWARD, 255);

				return HUNTER_CHASE;

			} else {

				motor_l_set(FORWARD, 255);

				motor_r_set(BACKWARD, 255);