Index: trunk/code/behaviors/formation_control/circle/circle.c
===================================================================
--- trunk/code/behaviors/formation_control/circle/circle.c	(revision 1625)
+++ trunk/code/behaviors/formation_control/circle/circle.c	(revision 1626)
@@ -6,8 +6,48 @@
 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;
 
 
+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();
+	}
+}
+
+
 /*
 	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.
@@ -83,6 +123,9 @@
 }
 
 
+
+/*~~~~~~~~~~~~~ NEED document
+*/
 int correctTurn(void)
 {
 	int bomNum = 0;
@@ -91,9 +134,9 @@
 	usb_puti(bomNum);
 	if(bomNum == 4)
 	{	timeout = 0;
-		motor_l_set(1, 200);
-		motor_r_set(1, 200);
-		return 0;
+		//motor_l_set(1, 200);
+		//motor_r_set(1, 200);
+		return 1; //<--------------------------------------------1 and 0 are switched
 	}
 	else
 	{
@@ -116,9 +159,12 @@
 			motor_r_set(FORWARD, 200);
 		}
 	}
-	return 1;
+	return 0;
 }
 
+
+/*~~~~~~~~~~~~~~ NEED document
+*/
 void correctApproach(void)
 {
 	int bomNum = 0;
@@ -147,7 +193,10 @@
 	}
 }
 
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+/*
+  Start turning  until the front receive the MAx bom reading
+*/
 void turn_to_beacon(int max){
 	if (max>-1 && max<16){
 		int index = (max+12)%16;
@@ -159,6 +208,11 @@
 	}
 }
 
+
+/*
+	Start turning its fron to the MAx bom reading
+	But it won't stop by itself.
+*/
 void turn_to_beacon2(int max){				// like the previous but no stop() call'
 
 
@@ -174,6 +228,13 @@
 	}
 }
 
+
+
+
+/*
+	Turn towards the MAX bom direction  
+	until the front receive the MAx bom reading
+*/
 void orient(void){
 	int max_index = -1;
 	while (max_index!=4) {
@@ -184,6 +245,14 @@
 		delay_ms(22);
 	}
 }
+
+
+/*
+	Turn towards the MAX bom direction  
+	until the front receive the MAx bom reading
+	
+	This function has less stop() so the robots run more smooth
+*/
 void orient2(void){
 	int max_index = -1;
 	while (max_index!=4) {
@@ -217,10 +286,11 @@
 		count++;
 	}
 }
-void go_straight_onefoot(void){				// essentially, another name for the above.  Should be removed.
-	go_straight();
-}
 
+
+/* 
+    BLINK the given number times
+*/
 void blink(int num) {
 	for(int i = 0; i<num; i++)
 	{
@@ -234,6 +304,8 @@
 
 
 
+
+
 int main(void)
 {
 	/* Initialize dragonfly board */
@@ -243,6 +315,7 @@
     	/* 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];
 	for (int i=0; i<16; i++) used[i] = 0;
@@ -269,13 +342,42 @@
 	
 	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. *******************TODO ***************
+		*/
+		
+		
+		// decide if its is center or not.
 		switch(state)
 		{
-			case 0:			// EDGE
+			/**********
+				if  EDGE /slave robots
+			*/
+			case 0:	
 				
-				bom_off();
-				while(1)
+				/*
+					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
+				*/
+				while(1)   
 				{
+					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)
@@ -288,7 +390,11 @@
 					}
 				}
 				
-				while(1)		// wait for the center bot to count all the robots and send a "done" packet
+				/*
+					1. Wait for DONE package 
+					2. The counting process is DONE
+				*/
+				while(1)		
 				{
 					orb_set_color(YELLOW);orb2_set_color(PURPLE);
 					packet_data=wl_basic_do_default(&data_length);
@@ -300,10 +406,13 @@
 					}
 				}
 				
+				
+				// COLOR afer DONE ---> MAGENTA
 				orb_set_color(MAGENTA);
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 				while(correctTurn()){				
 				}
+				forward(220);
 				//range_init();
 				
 				
@@ -319,20 +428,94 @@
 
 				stop();
 				orb_set_color(LIME);
-				//button1_wait ();			// code for lab1.
-				//go_straight_onefoot();
-				break;
 				
+				send_buffer[0]=CIRCLE_ACTION_ACK;
+				send_buffer[1]=robotid;	
+				wl_basic_send_global_packet(42,send_buffer,2);
 				
+				// 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)
+					{
+					
+					}
 				
-			case 1:			// BEACON
+					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
+			
+
+
+
+
+	
+			/***************
+			   if  The CENTER/BEACON/MASTER robot
+			*/	
+			case 1:			// BEACON /master /enter robots
 				switch(beacon_State) {
-				case 0:		// wait for a button to be pressed, then go to state 1
+				/*
+				   1. center  robots on wait for pressing button 1
+				*/
+				case 0:		
 					bom_on();
 					orb_set_color(PURPLE);
 					if(button1_click()) beacon_State=1;
-				break;	
+					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;
@@ -339,32 +522,104 @@
 					send_buffer[1]=get_robotid();
 					wl_basic_send_global_packet(42,send_buffer,2);
 					beacon_State=2;
-				break;
-				case 2: 	// find out how many robots there are
-				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);
-					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;
+					break;
+				/*
+					1. 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[packet_data[1]]=1;
+						}
 					}
-				}
-				if(waitingCounter >= 1500){
-					beacon_State=3;
-					//blink(robotsReceived);
-				}
-				break;
+					if(waitingCounter >= 1500){
+						beacon_State=3;
+					}
+					break;
+				/*
+					COUNTing is DONE.
+					Sending DONE package.
+				*/	
 				case 3:
-				orb_set_color(GREEN);
-				send_buffer[0]=CIRCLE_ACTION_DONE;
-				wl_basic_send_global_packet(42,send_buffer,2);
-				break;
-				}
-			break;
+					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; // <----------------------------------------------------------Echo wrote this "break". need to be checked. 
+				
+				/**
+				    NEED to be documented
+				*/
+				case 5:
+					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;
+			}
 		}
 	}
 
@@ -371,5 +626,5 @@
 	orb_set_color(RED);
 	while(1); /* END HERE */
 
-	return 0;
+	//return 0;
 }
Index: trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c
===================================================================
--- trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c	(revision 1625)
+++ trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c	(revision 1626)
@@ -87,7 +87,7 @@
 		if(master)
 		{
 		
-			if(sending_counter++>4)
+			if(sending_counter++>4)	// clock for switching the BOMs between the master and slave
 			{
 				switch_sending();
 				sending_counter = 0;
@@ -151,9 +151,9 @@
 				else
 				{
 					int max_bom = bom_get_max();
-					usb_puts("bom_get_max : ");
+					/*usb_puts("bom_get_max : ");
 					usb_puti(max_bom);
-					usb_puts("/n/r");
+					usb_puts("/n/r");*/
 				
 				
 				   if(max_bom == 8)
