Colony

/*** PROGRAM INFORMATION ***

	  This program assembles a group of robots into a circle and allows them movement

	within that formation.  Robots should be able to break formation and travel as a

	line, readjust in the face of obstacles, and reform if conditions are necessary.

	  The program begins waiting for a button press.  When pressed, a robot assumes the

	BEACON position, which means that it is the robot in the center of the circle and

	therefore in charge.  It then gathers robots around it by sending them commands.

	This code is executed using two finite state machines, nested inside one another.

	One controls the overall state of the robot (whether it is a BEACON, an EDGE, or

	WAITING, for example).

	  This code should be implemented so that most useful functions are built in to the

	machine.  For example, the BEACON robot should be able to call methods such as

	CircleUp() to gather robots around it, and Move(distance) to move the circle group

	all at once.

	  This Code is the property of the Carnegie Mellon Robotics Club and is being used

	to test formation control in a low-cost robot colony.  Thanks to all members of

	RoboClub, especially Colony president John Sexton and the ever-present Chris Mar.

	AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris, Joel Rey,

	 Reva Street, Alex Zirbel	 						*/

/*** TODO: ***

	- Transform the code into a method-based state machine that uses the procedural state

		machines, which are hardcoded and hard to edit, as a backup.

	- Implement a drive straight method for use in keeping the robots more accurate as a

		group.

	- Fix the approach method: good robots usually work well, but bad robots often have

		errors which might be avoidable with the use of error checking.

	- Make robots more robust: packages are often lost, which throws the entire procedural

		nature of the program off.

	- Consider using the center bot to check distances

	- More testing is always good and necessary.					*/

/*** BOT LOG ***

	4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.

	4-2-2010: BOT 7 and BOT 14 worked extremely well, no matter states.  BOT 1 started

		well, but malfunctioned later.						*/

/*** TERMINOLOGY ***

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

int END = 100;	

int WAITINGSTATE = 0;		/* Define some variables to keep track of the state machine.*/

int EDGE_CONTROL = 1;

int BEACON_CONTROL = 2;

int EDGE_MACHINE = 3;

int BEACON_MACHINE = 4;

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.

	wl_basic_send_global_packet(42,send_buffer,3);

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

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

	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.

    	/* Set the XBee channel to 24 - must be standard among robots */

	int centerid = 0;		// once the EDGE gets the first signal from a center, it stores who the center is.

	int used[17];			// 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.

	for (int i=0; i<17; i++) used[i] = 0;		// initially, no robots in the group.

	int data_length;		// keeps track of the length of wireless packets received.

	int state = WAITINGSTATE;

	int beacon_State=0;		// these variables keep track of the inner state machines in the procedural MACHINE states.

	int robotsReceived=0;		// an important variable that stores the size of the group.

	int offset = 20;		// offset for the approaching: how far off the rangefinders can be

	int direction = 4;		// keeps track of which way robots are facing relative to the center

	int distance=1000;		// 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 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:

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

					send2(CIRCLE_ACTION_ACK,robotid);

					state = EDGE_CONTROL;

				}

				if(button1_read())

				{

					send2(CIRCLE_CLAIM_CENTER, robotid); 	// becomes the center if button1 is clicked.

				}

			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:

			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:

			break;

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

			/*

				The MACHINE for the EDGE state.

			*/

			case 3:	

							if(time>50)

						if(packet_data != 0 && data_length==3 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid)

					/* Wait for the center bot to send a DONE packet; then turn to face the right direction. */

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

						}

					/* Turn until we reach the right direction (DIRECTION) */

						left(170);

						bom_refresh(BOM_ALL);

						if(bom_get_max() == direction)

						{

							stop();

							orb_set_color(YELLOW);

							edge_State = 8;

						}

					break;

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

					case 8:

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

							delay_ms(2000);

							edge_State = END;

						}

					break;

					/* Terminal. */

					case 100:

						stop();

						orb2_set_color(WHITE);

						while(1);

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

			   The MACHINE for the BEACON state

			case 4:

								bom_refresh(BOM_ALL);

								direction += 8;

								if(direction > 15) direction -= 16;

								delay_ms(20);

					/* 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 6:

						send2(CIRCLE_ACTION_DONE,robotid);

						bom_on();

						beacon_State = 7;

					break;

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

					case 7:

						orb_set_color(YELLOW);

						delay_ms(5000);

						// format: type of ack, speed, time.

						send3(CIRCLE_ACTION_FORWARD,200,2000);

						orb_set_color(BLUE);

						forward(200);

						delay_ms(2000);

						stop();

						beacon_State = END;

					break;

					case 100:

						stop();

						orb2_set_color(WHITE);

						while(1);

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

		}	// ends the main switch

	}		// ends the main while loop

	orb_set_color(RED);	// error, we should never break from the while loop!

	while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */

#define CIRCLE_ACTION_FORWARD 'F'

#define CIRCLE_CLAIM_CENTER 'C'