Index: circle.c
===================================================================
--- circle.c	(revision 1748)
+++ circle.c	(revision 1749)
@@ -1,56 +1,111 @@
+
+/*** 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	 						*/
+
+
 #include <dragonfly_lib.h>
 #include <wl_basic.h>
 #include <encoders.h>
 #include "circle.h"
 
-/*
-Current situation:
-	go to the center. ends when each edge robto send "I am here" message
+/*** TODO: ***
 
-TODO:
-  Center:
-    1. Make a move forward method/state to make the circle begin moving as a group.
+	- 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.					*/
 
-  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.
+/*** BOT LOG ***
 
-  In general:
-    More testing.  Especially with robots that are actually working.
-*/
+	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 ***
 
-/*
-	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.
+	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.
 
-	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, Joel
-*/
+	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.
 
-/* 
-TODO:
-	Used: Bots 1, 7
-		4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.
-	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
-*/
+	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.
 
-int EDGE = 0;
-int BEACON = 1;
+	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;
+
+
 int timeout = 0;
 int sending = 0;
 int stop2 = 0;
@@ -85,8 +140,8 @@
 	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.
+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){
@@ -133,7 +188,7 @@
 	send_buffer[0]=arg0;
 	send_buffer[1]=arg1;
 	send_buffer[2]=arg2;
-	wl_basic_send_global_packet(42,send_buffer,2);
+	wl_basic_send_global_packet(42,send_buffer,3);
 }
 
 /*
@@ -221,17 +276,14 @@
 
 
 
-
-
 //*****************************************************************************************************************************************************************************************
+//*****************************************************************************************************************************************************************************************
+//*****************************************************************************************************************************************************************************************
 
 
-
-
-
-
 /*
-	A double state machine.  The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading.
+	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.
 */
 int main(void)
 {
@@ -239,64 +291,113 @@
     	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 */
+    	/* Set the XBee channel to 24 - must be standard among robots */
 	wl_set_channel(24);
 
 	int robotid = get_robotid();
-	int centerid = 0;
-	int used[17];
-	for (int i=0; i<17; i++) used[i] = 0;
-	char send_buffer[2];
-	int data_length;
+	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.
 	unsigned char *packet_data=wl_basic_do_default(&data_length);
 	
-	
-	int state = EDGE;
-	int beacon_State=0;
+	int state = WAITINGSTATE;
+	int beacon_State=0;		// these variables keep track of the inner state machines in the procedural MACHINE states.
 	int edge_State=0;
+
 	int waitingCounter=0;
-	int robotsReceived=0;
-	int offset = 20;
+	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 time=0;
-	int direction = 4;	// keeps track of which way robots are facing relative to the center
+	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.
 	
-	if(wheel()>100)
-	{
-		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);
+
+				/*		
+				*******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
+				*/
 		
-		/*
-		*******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.  This is the main switch loop which governs the entire state of the robot.
+		/* This is the MAIN SWITCH LOOP, which governs the overall status of the robot. */
 		switch(state)
 		{
-			/**********
-				if  EDGE /slave robots
+
+
+			/*
+				The WAITINGSTATE.  This state constantly checks for wireless packets,
+				and updates its state as soon as it receives a signal.
 			*/
-			case 0:	
+			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:	
+
 				switch(edge_State)
 				{
 					/*
@@ -369,7 +470,7 @@
 							distance = get_distance();
 							delay_ms(14);
 							time+=14;
-							if(time>500)
+							if(time>50)
 							{
 								correctTurn();
 								time=0;
@@ -401,7 +502,7 @@
 					case 5:
 						orb2_set_color(YELLOW);
 						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)
+						if(packet_data != 0 && data_length==3 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid)
 						{
 							bom_off();
 							direction = packet_data[2];
@@ -410,14 +511,48 @@
 						}
 					break;
 
-					/* Blinks the direction it should turn. */
+					/* Wait for the center bot to send a DONE packet; then turn to face the right direction. */
 					case 6:
-						slowblink(direction);
-						edge_State = 7;
+						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;
+						}
 					break;
 
+					/* Turn until we reach the right direction (DIRECTION) */
 					case 7:
+						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();
 						orb_set_color(GREEN);
+						orb2_set_color(WHITE);
+						while(1);
 					break;
 
 
@@ -432,19 +567,15 @@
 
 
 
-	
-//*****************************************************************************************************************************************************************************************
-//*****************************************************************************************************************************************************************************************			
-//*****************************************************************************************************************************************************************************************
+//***********************************************************************************************************************************************************************************
 
 
 
 
-
 			/*
-			   The CENTER/BEACON/MASTER robot
+			   The MACHINE for the BEACON state
 			*/	
-			case 1:
+			case 4:
 				switch(beacon_State) 
 				{
 
@@ -544,27 +675,57 @@
 									}
 								}
 
+								bom_refresh(BOM_ALL);
 								direction = bom_get_max();
-								if(direction >= 16) direction -= 16;
+								direction += 8;
+								if(direction > 15) direction -= 16;
 								send3(CIRCLE_ACTION_GOTYOU, i, direction);
-								delay_ms(2000);
+								delay_ms(20);
 							}
 						}
 						beacon_State = 6;
 					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 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;
+
 					/* Terminal. */
-					case 6:
+					case 100:
+						stop();
 						orb_set_color(GREEN);
+						orb2_set_color(WHITE);
+						while(1);
 					break;
 				}
 			break;
+
+//***********************************************************************************************************************************************************************************
 			
-		}
-	}
+		}	// ends the main switch
+	}		// ends the main while loop
 
-	orb_set_color(RED);
-	while(1); /* END HERE */
+	orb_set_color(RED);	// error, we should never break from the while loop!
 
-	//return 0;
+	while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */
 }
+
+
