Index: circle.c
===================================================================
--- circle.c	(revision 1634)
+++ circle.c	(revision 1635)
@@ -12,7 +12,7 @@
 int desired_max_bom;
 int bom_max_counter;
 
-
+//Last used 12,13,7(BOM)
 void set_desired_max_bom(int desired_angle)
 {
 	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;
@@ -269,8 +269,10 @@
 	
 	int state = EDGE;
 	int beacon_State=0;
+	int edge_State=0;
 	int waitingCounter=0;
 	int robotsReceived=0;
+	int offset = 20, time=0;
 	
 	if(wheel()<100)
 	{
@@ -282,7 +284,7 @@
 	}
 	
 	int distance=1000;						// how far away to stop.
-	int onefoot=300, speed=250;				// one foot is 490 for bot 1; one foot is 200 for bot6
+	int onefoot=250, speed=250;				// one foot is 490 for bot 1; one foot is 200 for bot6
 	
 	while(1)
 	{
@@ -312,67 +314,81 @@
 				if  EDGE /slave robots
 			*/
 			case 0:	
-				
-				/*
-					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)   
+				switch(edge_State)
 				{
-					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)
-					{
-						send_buffer[0]=CIRCLE_ACTION_ACK;
-						send_buffer[1]=robotid;
+					/*
+						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
+					*/
+					case 0:   
+						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)
+						{
+							send_buffer[0]=CIRCLE_ACTION_ACK;
+							send_buffer[1]=robotid;
+							
+							wl_basic_send_global_packet(42,send_buffer,2);
+							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);
+						packet_data=wl_basic_do_default(&data_length);
+						//wl_basic_send_global_packet(42,send_buffer,2);
 						
-						wl_basic_send_global_packet(42,send_buffer,2);
-						break;
-					}
-				}
-				
-				/*
-					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);
-					wl_basic_send_global_packet(42,send_buffer,2);
+						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
+						{
+							edge_State=2;
+						}
+					break;
 					
-					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
-					{
-						break;
-					}
+					case 2:
+						// COLOR afer DONE ---> MAGENTA
+						orb_set_color(MAGENTA);
+						correctTurn();			// turn to face the beacon
+						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);
+							//correctApproach();
+							distance = get_distance();
+							delay_ms(14);
+							time+=14;
+							if(time>500){
+								correctTurn();
+								time=0;
+							}
+						}
+							
+						stop();
+						orb_set_color(LIME);
+						edge_State=3;
+					break;
+					
+					case 3:
+					//do nothing
+					break;
 				}
-				
-				
-				// COLOR afer DONE ---> MAGENTA
-				orb_set_color(MAGENTA);
-//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-				correctTurn();			// turn to face the beacon
-				forward(220);
-				//range_init();
-				
-				
-				distance = get_distance();
-				while (distance>=onefoot || distance==0)
-				{
-					if(distance==0)
-						orb_set_color(WHITE);
-					//correctApproach();
-					distance = get_distance();
-					delay_ms(14);
-				}
-
-				stop();
-				orb_set_color(LIME);
-				
-				send_buffer[0]=CIRCLE_ACTION_ACK;
+				/*send_buffer[0]=CIRCLE_ACTION_ACK;
 				send_buffer[1]=robotid;	
 				wl_basic_send_global_packet(42,send_buffer,2);
 				
@@ -406,8 +422,8 @@
 								/*usb_puts("bom_get_max : ");
 							usb_puti(max_bom);
 							usb_puts("/n/r");*/
+					/*	
 						
-						
 							if(max_bom == 8)
 							{	
 								orb2_set_color(BLUE);
@@ -434,7 +450,7 @@
 				delay_ms(10);
 				
 				} //end while
-				break;
+				*/break;
 				
 				// END for EDGE robots
 			
@@ -449,7 +465,7 @@
 			case 1:			// BEACON /master /enter robots
 				switch(beacon_State) {
 				/*
-				   1. center  robots on wait for pressing button 1
+				   0. center  robots on wait for pressing button 1
 				*/
 				case 0:		
 					bom_on();
@@ -467,7 +483,7 @@
 					beacon_State=2;
 					break;
 				/*
-					1. Count the number of the EDGE robots 
+					2. Count the number of the EDGE robots 
 					*******NOTE: at most  1500  times of loop ******
 				*/
 				case 2: 	
@@ -485,7 +501,7 @@
 							used[packet_data[1]]=1;
 						}
 					}
-					if(waitingCounter >= 1500){
+					if(waitingCounter >= 750){
 						beacon_State=3;
 					}
 					break;
@@ -504,7 +520,7 @@
 					Wait for all the robots to get to right distance/position 
 				*/
 				case 4: 
-					orb1_set_color(YELLOW);
+					/*orb1_set_color(YELLOW);
 					orb2_set_color(WHITE);
 					
 					int numOk = 0;
@@ -518,7 +534,7 @@
 						}
 					}
 					
-					beacon_State = 5;
+					beacon_State = 5;*/
 					break; // <----------------------------------------------------------Echo wrote this "break". need to be checked. 
 				
 				/**
@@ -525,7 +541,7 @@
 				    NEED to be documented
 				*/
 				case 5:
-					orb_set_color(BLUE);
+					/*orb_set_color(BLUE);
 					// clock for switching the BOMs between the master and slave
 					if(sending_counter++>4)	
 					{
@@ -562,7 +578,7 @@
 						else bom_max_counter = 0;
 						
 					}
-					
+					*/
 					break;
 			}
 		}
