Colony

Joel Rey, Reva Street, Alex Zirbel

 	*/

   -More testing is always good and necessary.					

*/

       	started well, but malfunctioned later.

        */

  WAITINGSTATE:

   The robot waits to be given a signal to do something.  Wireless is on,

in case the robot is called on to turn into an EDGE.  The color should be LIME

or YELLOW-GREEN.

  BEACON_CONTROL:

   The code that executes commands when a robot is turned to BEACON mode.  This

code may run predefined methods for simplicity.  One goal is to make these

methods change the robot turn to to BEACON_MACHINE mode for a while, and then

return to the CONTROL code where they left off.

  EDGE_CONTROL:

   Like BEACON_CONTROL, executes whatever orders are required of the robot as an

EDGE.

  BEACON_MACHINE:

   A hardcoded list of functions which the robot is capable of running through.

Consists of a finite state machine, where the robot executes a set of commands

in a procedural manner and then returns to wherever it was in the control code.

  EDGE_MACHINE:

   Like the BEACON_MACHINE, but contains the same sort of procedural information

for EDGE robots.

  END:

   A terminal state of the machine, where the robot just sits and waits.  The

color should be GREEN and WHITE.

  TYPES OF WIRELESS PACKETS:

   CIRCLE_ACTION_EXIST 'E'

   CIRCLE_ACTION_POSITION 'P'

   CIRCLE_ACTION_ACK 'A'

 	A general acknowledgement package.

   CIRCLE_ACTION_DONE 'D'

 	Used by robots to tell when they have finished their action.

   CIRCLE_ACTION_GOTYOU 'G'

 	Used by the BEACON to tell a robot when it has been checked off.

 	At this point, the EDGE has been recognized.  Used for times when

 	all EDGE robots have to communicate to the center via the spam method.

   CIRCLE_ACTION_FORWARD 'F'

 	The BEACON tells the rest of the robots to move forward.

   CIRCLE_CLAIM_CENTER 'C'

 	Sent out by a robot when it takes over as BEACON.

 	*/

/* Define some variables to keep track of the state machine.*/

int WAITINGSTATE = 0;

int TURNL = 4;

int TURNR = 5;

// keep track of the speed and duration of group movements.

int speed = 20;

int duration = 2;

// set the motors to this forward speed.

void forward(int speed)

{		

// turn left at this speed.

void left(int speed)

{	

void right(int speed)

{

// stop() is better than motors_off(), which creates a slight delay when

// reactivating the motors.  Stop() is faster.

void stop(void)

{

	motor_l_set(BACKWARD,0);

void setforward(int spd1, int spd2)

{

void backward(int speed)

{

// takes an averaged reading of the front rangefinder

int get_distance(void)

{

	// kk sets this to 5 readings.

	int temp,distance,kk=5;			

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

	{

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

/*

   Turns the robot slowly to the right until it reaches the BOM reading goal.

   More stable code than what was implemented ealier, with smart turning,

   but slower.

*/

	orb1_set_color(BLUE);	// BLUE and PURPLE

   BLINK the given number times

   BLINK slowly the given number times

/*

   A method for the higher-level code for the BEACON.  The beacon can make

   any of the preprogrammed commands, and this code sends the packet and

   transitions the robots correctly.

*/

/*

   A method for the higher-level code for the BEACON.  The beacond sends

   not only the command, but also the speed and duration for which the

   (movement) command is to be executed.

*/

void orderMove(int action, int newSpeed, int newDuration)

{

	currentPos++;

	speed = newSpeed;

	duration = newDuration;

	send2(CIRCLE_EXECUTE, action);

	state = 20 + action;

}

/*

   Turns off the motors, sends an EXECUTE packet, and blinks green and white

   forever.

*/

	motors_off();

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

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

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

   A state machine with five states.  The robot starts out in WAITINGSTATE mode,

from which it recieves a signal of some sort and moves to a different state.

	// once the EDGE gets the first signal from a center, it stores who the 	// center is.

	int centerid = 0;

	// stores a list of bots which are in the group by storing a "1" in the

	// array if the robot of that index is in the group.	

	int used[17];

	int numOk;

	// initially, no robots in the group.

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

		used[i] = 0;

	// keeps track of the length of wireless packets received.

	int data_length;

	// these variables keep track of the inner state machines in the

	//  procedural MACHINE states.	

	int beacon_State=0;

	// an important variable that stores the size of the group.

	int robotsReceived=0;

	// offset for the approaching: how far off the rangefinders can be

	int offset = 20;

	// keeps track of which way robots are facing relative to the center

	int direction = 4;

	// how far away the robot is.  Initialized to a large value to ensure

	// that the robot doesn't think it is already the right distance away.

	int distance=1000;

	int onefoot = 250;	// how far away to stop.

	/***EXPECTED MOVES***

	   (OUT OF DATE.  Will be updated once changes have been made.)

	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

		*/

		/*

		  This is the MAIN SWITCH LOOP, which governs the overall

		  status of the robot.

		 */

		/*

		The WAITINGSTATE.  This state constantly checks for wireless

		packets,

		and updates its state as soon as it receives a signal.

		*/

		case 0:

			orb_set_color(YELLOW);

			packet_data=wl_basic_do_default(&data_length);

			if(packet_data != 0 && data_length>=2

				&& packet_data[0]==CIRCLE_CLAIM_CENTER)

			{

				centerid = packet_data[1];

				state = 1;

			}

			if(button1_read())

			{

				// becomes the center if button1 is clicked.

				send2(CIRCLE_CLAIM_CENTER, robotid); 	

				state = 2;

			}

		break;

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

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

		/*

		The CONTROL for the EDGE state.  This sets a certain procedure

		to follow, in the form of simple

		commands, for a robot to follow if it is set to an EDGE.

		*/

		case 1:

			orb_set_color(CYAN);

			orb1_set_color(YELLOW);

			int command = -1;

			packet_data=wl_basic_do_default(&data_length);

			if(packet_data != 0 && data_length>=2 &&

			   packet_data[0]==CIRCLE_EXECUTE)

			{

				command = packet_data[1];

			}

			if(command != -1)

			{

				edge_State = 0;

				switch(command)

				case 0:

					state = 10; break;

				case 1:

					state = 11; break;

				case 2:

					state = 12; break;

				case 3:

					state = 13; break;

				case 4:

					state = 14; break;

				case 5:

					state = 15; break;

				case 100:

					terminate(); break;

			}

		break;

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

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

		/*

		The CONTROL for the BEACON state.  This sets a certain procedure

		to follow, in the form of simple commands, for a robot to follow

		if it is set to a BEACON.

		*/

		case 2:

			orb_set_color(PURPLE);

			beacon_State = 0;

			switch(currentPos)

			{

			case 0:

				order(COUNT);	break;

			case 1:

				order(CIRCLEUP); break;

			case 2:

				order(ORIENT); break;

			case 3:

				orderMove(TURNL,20,2); break;

			case 4:

				terminate(); break;

			}

		break;

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

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

	/* The following states are MACHINE states for the EDGE robot. */

		/*

			EDGE on COUNT

		*/

		case 10:	

			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. Done for now: display green.

				*/

			case 0:

				bom_off();

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

					centerid = packet_data[1];

					send2(CIRCLE_ACTION_ACK,robotid);

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

				// keep sending the packet until we get a

				// response

				send2(CIRCLE_ACTION_ACK,robotid);

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length>=2 &&

				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

				   packet_data[1] == robotid)

				{

					edge_State=2;

				}

			break;

			// wait for the second, general, done packet.

			case 2:	

				orb_set_color(YELLOW);

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length>=2 &&

				   packet_data[0]==CIRCLE_ACTION_DONE &&

				   packet_data[1] == centerid)

				{

					state = 1;

			}

		break;

		/* The CIRCLEUP command for EDGE */

		case 11:

			switch(edge_State)

			{

			case 0:

				// COLOR afer DONE ---> MAGENTA

				orb_set_color(MAGENTA);

				// turn to face the beacon

				faceFront();

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

					distance = get_distance();

					delay_ms(14);

					time+=14;

					if(time>50)

					{

						faceFront();

					}

				}

				stop();

				orb_set_color(GREEN);

				send2(CIRCLE_ACTION_ACK, robotid);

				stop();

				state = 1;

			break;

			}

		break;

		/* An ORIENT series of steps for the EDGE robot. */

		case 12:

			switch(edge_State)

			{

			// waits for a packet to tell it to turn on the bom.

			case 0:

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length==2 &&

				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

				   packet_data[1] == robotid)

					bom_on();

					orb_set_color(ORANGE);

					send2(CIRCLE_ACTION_ACK,centerid);

					edge_State = 1;

				}

			break;

			// waits for a packet to tell it that it has been

			// received.

			case 1:

				orb2_set_color(YELLOW);

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length==3 &&

				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&

				   packet_data[1] == robotid)

				{

					bom_off();

					direction = packet_data[2];

					orb_set_color(YELLOW);

					edge_State = 2;

				}

			break;

			/*

			 Wait for the center bot to send a DONE packet; then

			 turn to face the right direction.

			*/

			case 2:

				orb_set_color(GREEN);

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length>=2 &&

				   packet_data[0]==CIRCLE_ACTION_DONE)

				{

					orb_set_color(WHITE);

					orb2_set_color(CYAN);

					edge_State = 3;

				}

			break;

			/* Turn until we reach the right direction */

			case 3:

				aboutFace(direction);

				stop();

				orb_set_color(YELLOW);

				send2(CIRCLE_ACTION_DONE,robotid);

				state = 1;

			break;

			}

		break;

		/* The DRIVE steps for the EDGE robot */

		case 13:

			switch(edge_State)

			{

			/* Wait for the command to move forward. */

			case 0:

				packet_data=wl_basic_do_default(&data_length);

				if(packet_data != 0 && data_length>=3 &&

				   packet_data[0]==CIRCLE_ACTION_FORWARD)

				{

					orb_set_color(BLUE);

					forward(packet_data[1]*10);

					delay_ms(packet_data[2]*1000);

					stop();

					state = 1;

				}

			break;

			}

		break;

		/* The TURNL steps for the EDGE robot */

		case 14:

			orb_set_color(RED);

			/* Wait for specifications for the turn. */

			packet_data=wl_basic_do_default(&data_length);

			if(packet_data != 0 && data_length>=3 &&

			   packet_data[0]==CIRCLE_ACTION_TURN)

			{

				orb_set_color(BLUE);

				left(packet_data[1]*10);

				delay_ms(packet_data[2]*1000);

				stop();

				state = 1;

			}

		break;

		/* The TURNR steps for the EDGE robot */

		case 15:

			orb_set_color(RED);

			/* Wait for specifications for the turn. */

			packet_data=wl_basic_do_default(&data_length);

			if(packet_data != 0 && data_length>=3 &&

			   packet_data[0]==CIRCLE_ACTION_TURN)

			{

				orb_set_color(BLUE);

				right(packet_data[1]*10);

				delay_ms(packet_data[2]*1000);

				stop();

				state = 1;

			}

		break;

	// END for EDGE robots

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

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

		/*

		   The MACHINE for the BEACON state

		*/

		/* the COUNT code for the BEACON */

		case 20:

			switch(beacon_State) 

			{

			/* 0. center  robots on wait for pressing button 1 */

			case 0:

				bom_on();

				orb_set_color(BLUE);

				robotsReceived = 0;

				beacon_State=1;

			break;	

			/* 1. Send EXIST package to EDGE robots */

			case 1:

				orb_set_color(RED);

				send2(CIRCLE_ACTION_EXIST,robotid);

				beacon_State=2;

			break;

			/* 2. Count the number of the EDGE robots

			 *******NOTE: at most  1000  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 list seen if you haven't

					// gotten an ACK from this robot

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

					{

						robotsReceived++;

						used[packet_data[1]] = 1;

						usb_puts("Added: ");

						usb_puti(packet_data[1]);

						usb_puts("\r\n");

					}

					// NEW: sends a packet to each robot it

					// receives telling them to be done.

					send2(CIRCLE_ACTION_GOTYOU,

						packet_data[1]);

				if(waitingCounter >= 300)

				{

					beacon_State=3;

				}

			break;

			/* COUNTing is DONE.  Sending DONE package. */	

			case 3:

				blink(robotsReceived);

				orb_set_color(GREEN);

				send2(CIRCLE_ACTION_DONE, robotid);

				state = 2;

			}

		break;

		/* The CIRCLEUP method for BEACON */

		case 21:

			switch(beacon_State)

			{

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

			case 0: 

				left(170);

				orb1_set_color(YELLOW);

				orb2_set_color(WHITE);

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

					}

				}

				state = 2;

			}

		break;

		/* The ORIENT code for the beacon */

		case 22:

			switch(beacon_State)

			{

			/* Turns all the robots in the same direction */

			case 0:

				stop();

				bom_off();

				orb_set_color(ORANGE);

				// for each robot, tells them to turn their bom

				// on, then tells them which way to face.

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

				    if(used[i] == 1)

				    {

					send2(CIRCLE_ACTION_GOTYOU, i);

					// waits for a response so it knows the

					// BOM is on.

					while(1)

					{

					    orb_set_color(RED);

					    orb2_set_color(WHITE);

					    packet_data=wl_basic_do_default(

						&data_length);

					    if(packet_data!=0 && data_length>=2

						&& packet_data[0]==

						CIRCLE_ACTION_ACK)

					    {

						break;

					    }

					}

					delay_ms(20);

					bom_refresh(BOM_ALL);

					direction = bom_get_max();

					direction += 8;

					if(direction > 15) direction -= 16;

					delay_ms(20);

					send3(CIRCLE_ACTION_GOTYOU, i,

					     direction);

					delay_ms(20);

				    }

				beacon_State = 1;

			break;

			/*

			Sends a DONE packet to signify that it has read in all

			the robots' directions and sent packets.

			Edge robots should now turn to face the right direction.

			*/

			case 1:

				send2(CIRCLE_ACTION_DONE,robotid);

				bom_on();

				beacon_State = 2;

			break;

			case 2:

				numOk = 0;

				while(numOk < robotsReceived)

				{

					orb_set_color(ORANGE);

					packet_data=wl_basic_do_default(

					   &data_length);

					if(packet_data!=0 && data_length>=2 &&

					   packet_data[0]==CIRCLE_ACTION_DONE)

					{

						numOk++;

					}

				}

				state = 2;

			}

		break;

		/* The DRIVE code for the beacon */

		case 23:

			switch(beacon_State)

			{

			/* Tells the robots to move forward and moves itself. */

			case 0:

				orb_set_color(YELLOW);

				delay_ms(50);

				// format: type of ack, speed divided by 10,

				// time in seconds.

				send3(CIRCLE_ACTION_FORWARD,speed,duration);

				orb_set_color(BLUE);

				forward(speed*10);

				delay_ms(duration*1000);

				stop();

				beacon_State = 1;

			/* Terminal. */

			case 1:

				stop();

				state = 2;

			break;

			}

		break;

		/* The TURNL code for the beacon */

		case 24:

			orb_set_color(YELLOW);

			delay_ms(50);

			// format: type of ack, speed divided by 10,

			// time in seconds.

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

				send3(CIRCLE_ACTION_TURN,speed,duration);

			orb_set_color(BLUE);

			left(speed*10);

			delay_ms(duration*1000);

			stop();

			state = 2;

		break;

		/* The TURNR code for the beacon */

		case 25:

			orb_set_color(YELLOW);

			delay_ms(50);

			// format: type of ack, speed divided by 10,

			// time in seconds.

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

				send3(CIRCLE_ACTION_TURN,speed,duration);

			orb_set_color(BLUE);

			right(speed*10);

			delay_ms(duration*1000);

			stop();

			state = 2;

		break;

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

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

	// error, we should never break from the while loop!

	orb_set_color(RED);	

	/*

	 END HERE, just in case something happened.

	 This way we can see the red orb.

	*/

	while(1);