Index: trunk/code/behaviors/formation_control/circle/circle.c
===================================================================
--- trunk/code/behaviors/formation_control/circle/circle.c	(revision 1617)
+++ trunk/code/behaviors/formation_control/circle/circle.c	(revision 1618)
@@ -5,7 +5,9 @@
 
 int EDGE = 0;
 int BEACON = 1;
+int timeout = 0;
 
+
 /*
 	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.
@@ -34,6 +36,8 @@
 		*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);
@@ -77,6 +81,73 @@
 	else 
 		return 0;
 }
+
+
+int correctTurn(void)
+{
+	int bomNum = 0;
+	bom_refresh(BOM_ALL);
+	bomNum = bom_get_max();
+	usb_puti(bomNum);
+	if(bomNum == 4)
+	{	timeout = 0;
+		motor_l_set(1, 200);
+		motor_r_set(1, 200);
+		return 0;
+	}
+	else
+	{
+		if(bomNum == -1){
+			timeout++;
+			if(timeout > 500000)
+			{
+				motor_r_set(FORWARD, 210);
+				motor_l_set(BACKWARD, 190);
+			}
+		}
+		else if((bomNum >= 12) || (bomNum < 4))
+		{
+			motor_l_set(FORWARD, 200);timeout = 0;
+			motor_r_set(BACKWARD, 200);
+		}
+		else
+		{
+			motor_l_set(BACKWARD, 200);timeout = 0;
+			motor_r_set(FORWARD, 200);
+		}
+	}
+	return 1;
+}
+
+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);
+	}
+}
+
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 void turn_to_beacon(int max){
 	if (max>-1 && max<16){
 		int index = (max+12)%16;
@@ -102,6 +173,7 @@
 		else left(170);
 	}
 }
+
 void orient(void){
 	int max_index = -1;
 	while (max_index!=4) {
@@ -122,6 +194,7 @@
 		delay_ms(22);
 	}
 }
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 void go_straight(void){						// drives forward a hardcoded distance. May not be useful.
 	forward(200);
 	encoder_rst_dx(LEFT);
@@ -164,12 +237,10 @@
 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 */
+    	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 robotid = get_robotid();
@@ -202,10 +273,10 @@
 		{
 			case 0:			// EDGE
 				
-				orb_set_color(GREEN);
+				bom_off();
 				while(1)
 				{
-					orb_set_color(YELLOW);
+					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)
 					{
@@ -219,7 +290,7 @@
 				
 				while(1)		// wait for the center bot to count all the robots and send a "done" packet
 				{
-					orb_set_color(RED);
+					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);
 					
@@ -229,13 +300,12 @@
 					}
 				}
 				
-				orb_set_color(GREEN);
-				
-				orient();
-				forward(speed);
+				orb_set_color(MAGENTA);
+//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+				while(correctTurn()){				
+				}
 				//range_init();
 				
-				orb_set_color(BLUE);
 				
 				distance = get_distance();
 				while (distance>=onefoot || distance==0)
@@ -242,17 +312,15 @@
 				{
 					if(distance==0)
 						orb_set_color(WHITE);
+					//correctApproach();
 					distance = get_distance();
-					orient2();
-					forward(speed);
 					delay_ms(14);
 				}
+
 				stop();
-				orient();
-				
+				orb_set_color(LIME);
 				//button1_wait ();			// code for lab1.
 				//go_straight_onefoot();
-				stop();
 				break;
 				
 				
@@ -266,6 +334,7 @@
 					if(button1_click()) beacon_State=1;
 				break;	
 				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);
@@ -273,11 +342,12 @@
 				break;
 				case 2: 	// find out how many robots there are
 				waitingCounter++;
-				orb_set_color(ORANGE);
+				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(WHITE);
+					orb_set_color(RED);orb2_set_color(BLUE);
 					if(used[packet_data[1]]==0){		//only add to robots seen if you haven't gotten an ACK from this robot
 						robotsReceived++;
 						used[packet_data[1]]=1;
@@ -289,7 +359,7 @@
 				}
 				break;
 				case 3:
-				orb_set_color(PURPLE);
+				orb_set_color(GREEN);
 				send_buffer[0]=CIRCLE_ACTION_DONE;
 				wl_basic_send_global_packet(42,send_buffer,2);
 				break;
