Colony - / - Diff - Robotics Club

Revision 1807

     student representative Chris Mar.
        AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris,
     Joel Rey, Reva Street, Alex Zirbel	 						*/
     Joel Rey, Reva Street, Alex Zirbel
      	*/
     #include <dragonfly_lib.h>
-...
        -Consider using the center bot to check distances
        -More testing is always good and necessary.					*/
        -More testing is always good and necessary.
     */
     /*** BOT LOG ***
 -1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.
 -2-2010: BOT 7 and BOT 14 worked extremely well, no matter states.  BOT 1
            	started well, but malfunctioned later.					        */
            	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.
       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.
       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.
       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.
       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.
       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.
       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.		*/
       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.
      	*/
     /* Define some variables to keep track of the state machine.*/
     int END = 100;
     int WAITINGSTATE = 0;		/* Define some variables to keep track of the state machine.*/
     int WAITINGSTATE = 0;
     int EDGE_CONTROL = 1;
     int BEACON_CONTROL = 2;
     int EDGE_MACHINE = 3;
-...
     int CIRCLEUP = 1;
     int ORIENT = 2;
     int DRIVE = 3;
     int TURNL = 4;
     int TURNR = 5;
     int currentPos = 0;
     int state = 0;
     // keep track of the speed and duration of group movements.
     int speed = 20;
     int duration = 2;
     int timeout = 0;
     int sending = 0;
-...
+    	}
+    }
     void forward(int speed){			// set the motors to this forward speed.
     // set the motors to this forward speed.
     void forward(int speed)
+    {
     	motor_l_set(FORWARD,speed);
     	motor_r_set(FORWARD,speed);
+    }
     void left(int speed){				// turn left at this speed.
     // turn left at this speed.
     void left(int speed)
+    {
     	motor_l_set(BACKWARD,speed);
     	motor_r_set(FORWARD,speed);
+    }
     void right(int speed){
     void right(int speed)
+    {
     	motor_l_set(FORWARD,speed);
     	motor_r_set(BACKWARD,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.
     // stop() is better than motors_off(), which creates a slight delay when
     // reactivating the motors.  Stop() is faster.
     void stop(void)
+    {
     	motor_l_set(BACKWARD,0);
     	motor_r_set(FORWARD,0);
+    }
     void setforward(int spd1, int spd2){
     void setforward(int spd1, int spd2)
+    {
     	motor_l_set(FORWARD,spd1);
     	motor_r_set(FORWARD,spd2);
+    }
     void backward(int speed){
     void backward(int speed)
+    {
     	motor_l_set(BACKWARD, speed);
     	motor_r_set(BACKWARD, speed);
+    }
     int get_distance(void){				// takes an averaged reading of the front rangefinder
     	int temp,distance,kk=5;			// kk sets this to 5 readings.
     // takes an averaged reading of the front rangefinder
     int get_distance(void)
+    {
     	// kk sets this to 5 readings.
     	int temp,distance,kk=5;
     	distance =0;
     	for (int i=0; i<kk; i++){
     	for (int i=0; i<kk; i++)
+    	{
     		temp = range_read_distance(IR2);
     		if (temp == -1)
+    		{
-...
+    }
     /*
     	Orients the robot so that it is facing the beacon (or the broadcasting BOM).
        Orients the robot so that it is facing the beacon (or the broadcasting BOM).
     */
     void faceFront(void)
+    {
-...
     	return;
+    }
     /*
        Turns the robot slowly to the right until it reaches the BOM reading goal.
        More stable code than what was implemented ealier, with smart turning,
        but slower.
     */
     void aboutFace(int goal)
+    {
     	int bomNum = -1;
     	int speed = 170;	// speed with which to turn
     	orb1_set_color(BLUE);			// BLUE and PURPLE
     	orb1_set_color(BLUE);	// BLUE and PURPLE
     	while(bomNum != goal)
+    	{
-...
     /*
         BLINK the given number times
        BLINK the given number times
     */
     void blink(int num)
+    {
-...
+    }
     /*
         BLINK slowly the given number times
        BLINK slowly the given number times
     */
     void slowblink(int num)
+    {
-...
     	orb_set_color(ORB_OFF);
+    }
     /*
        A method for the higher-level code for the BEACON.  The beacon can make
        any of the preprogrammed commands, and this code sends the packet and
        transitions the robots correctly.
     */
     void order(int action)
+    {
     	currentPos++;
-...
     	state = 20 + action;
+    }
     /*
        A method for the higher-level code for the BEACON.  The beacond sends
        not only the command, but also the speed and duration for which the
        (movement) command is to be executed.
     */
     void orderMove(int action, int newSpeed, int newDuration)
+    {
     	currentPos++;
     	speed = newSpeed;
     	duration = newDuration;
     	send2(CIRCLE_EXECUTE, action);
     	state = 20 + action;
+    }
     /*
        Turns off the motors, sends an EXECUTE packet, and blinks green and white
        forever.
     */
     void terminate(void)
+    {
     	motors_off();
     	send2(CIRCLE_EXECUTE, 100);
     	orb_set_color(GREEN);
     	orb2_set_color(WHITE);
-...
+    }
     //******************************************************************************
     //******************************************************************************
     //******************************************************************************
     //*****************************************************************************************************************************************************************************************
     //*****************************************************************************************************************************************************************************************
     //*****************************************************************************************************************************************************************************************
     /*
     	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.
        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)
+    {
-...
     	wl_set_channel(24);
     	int robotid = get_robotid();
     	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.
     	// once the EDGE gets the first signal from a center, it stores who the 	// center is.
     	int centerid = 0;
     	// 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.
     	int used[17];
     	int numOk;
     	// initially, no robots in the group.
     	for (int i=0; i<17; i++)
     		used[i] = 0;
     	// keeps track of the length of wireless packets received.
     	int data_length;
     	unsigned char *packet_data=wl_basic_do_default(&data_length);
     	int beacon_State=0;		// these variables keep track of the inner state machines in the procedural MACHINE states.
     	// these variables keep track of the inner state machines in the
     	//  procedural MACHINE states.
     	int beacon_State=0;
     	int edge_State=0;
     	int waitingCounter=0;
     	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
     	// an important variable that stores the size of the group.
     	int robotsReceived=0;
     	// offset for the approaching: how far off the rangefinders can be
     	int offset = 20;
     	int time=0;
     	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.
     	// keeps track of which way robots are facing relative to the center
     	int direction = 4;
     	// 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 distance=1000;
     	int onefoot = 250;	// how far away to stop.
     	while(1)
+    	{
     		bom_refresh(BOM_ALL);
     				/*
     				*******EXPECTED MOVES **********   (OUT OF DATE.  Will be updated once changes have been made.)
     				The designed movement:
 . one center robot, several edge robots are on;
 . center robots: button 1 is pressed;
 . center robots: send global package telling edges that he exists;
 . EDGE robots response with ACK.
 . EDGE robots wait for center robots to finish counting (DONE package)
 . EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center
     				*/
     	/***EXPECTED MOVES***
     	   (OUT OF DATE.  Will be updated once changes have been made.)
     	The designed movement:
 . one center robot, several edge robots are on;
 . center robots: button 1 is pressed;
 . center robots: send global package telling edges that he exists;
 . EDGE robots response with ACK.
 . EDGE robots wait for center robots to finish counting (DONE package)
 . 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. */
     		/*
     		  This is the MAIN SWITCH LOOP, which governs the overall
     		  status of the robot.
     		 */
     		switch(state)
+    		{
     			/*
     				The WAITINGSTATE.  This state constantly checks for wireless packets,
     				and updates its state as soon as it receives a signal.
     			*/
     			case 0:
     		/*
     		The WAITINGSTATE.  This state constantly checks for wireless
     		packets,
     		and updates its state as soon as it receives a signal.
     		*/
     		case 0:
     				orb_set_color(YELLOW);
     				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];
     			orb_set_color(YELLOW);
     			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];
     				state = 1;
+    			}
     					state = 1;
+    				}
     			if(button1_read())
+    			{
     				// becomes the center if button1 is clicked.
     				send2(CIRCLE_CLAIM_CENTER, robotid);
     				state = 2;
+    			}
     		break;
     				if(button1_read())
+    				{
     					send2(CIRCLE_CLAIM_CENTER, robotid); 	// becomes the center if button1 is clicked.
     					state = 2;
+    				}
     			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:
     			orb_set_color(CYAN);
     			orb1_set_color(YELLOW);
     			int command = -1;
     			packet_data=wl_basic_do_default(&data_length);
     			/*
     				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.
     			*/
     			if(packet_data != 0 && data_length>=2 &&
     			   packet_data[0]==CIRCLE_EXECUTE)
+    			{
     				command = packet_data[1];
+    			}
     			case 1:
     				orb_set_color(CYAN);
     				orb1_set_color(YELLOW);
     				int command = -1;
     				packet_data=wl_basic_do_default(&data_length);
     				if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_EXECUTE)
     			if(command != -1)
+    			{
     				edge_State = 0;
     				switch(command)
+    				{
     					command = packet_data[1];
+    				}
     				case 0:
     					state = 10; break;
     				if(command != -1)
+    				{
     					edge_State = 0;
     					switch(command)
+    					{
     						case 0:
     							state = 10; break;
     				case 1:
     					state = 11; break;
     						case 1:
     							state = 11; break;
     				case 2:
     					state = 12; break;
     						case 2:
     							state = 12; break;
     				case 3:
     					state = 13; break;
     						case 3:
     							state = 13; break;
     				case 4:
     					state = 14; break;
     						case 100:
     							terminate(); break;
+    					}
     				case 5:
     					state = 15; break;
     				case 100:
     					terminate(); break;
+    				}
+    			}
     			break;
     		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:
     			orb_set_color(PURPLE);
     			beacon_State = 0;
     			/*
     				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:
     				orb_set_color(PURPLE);
     				beacon_State = 0;
     				switch(currentPos)
+    				{
     					case 0:
     						order(COUNT);	break;
     			switch(currentPos)
+    			{
     			case 0:
     				order(COUNT);	break;
     					case 1:
     						order(CIRCLEUP); break;
     			case 1:
     				order(CIRCLEUP); break;
     					case 2:
     						order(ORIENT); break;
     			case 2:
     				order(ORIENT); break;
     					case 3:
     						order(DRIVE); break;
     			case 3:
     				orderMove(TURNL,20,2); break;
     					case 4:
     						terminate(); break;
+    				}
     			case 4:
     				terminate(); break;
+    			}
     			break;
     		break;
     //******************************************************************************
     //******************************************************************************
     //***********************************************************************************************************************************************************************************
     	/* The following states are MACHINE states for the EDGE robot. */
     			/* The following states are MACHINE states for the EDGE robot. */
     		/*
     			EDGE on COUNT
     		*/
     		case 10:
     			/*
     				EDGE on COUNT
     			*/
     			case 10:
     			switch(edge_State)
+    			{
     				/*
 . EDGE robots are on.
 . They are waiting for EXIST pacakage from the
     					Center robots
 . After they receive the package, they send ACK
     					package to center.
 . Done for now: display green.
     				*/
     			case 0:
     				bom_off();
     				orb1_set_color(YELLOW);
     				orb2_set_color(BLUE);
     				packet_data=wl_basic_do_default(&data_length);
     				switch(edge_State)
     				if(packet_data != 0 && data_length>=2 &&
     				   packet_data[0]==CIRCLE_ACTION_EXIST)
+    				{
     					/*
 . EDGE robots are on.
 . They are waiting for EXIST pacakage from the Center robots
 . After they receive the package, they send ACK package to center.
 . Done for now: display green.
     					*/
     					case 0:
     						bom_off();
     						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_EXIST)
+    						{
     							centerid = packet_data[1];
     					centerid = packet_data[1];
     							send2(CIRCLE_ACTION_ACK,robotid);
     					send2(CIRCLE_ACTION_ACK,robotid);
     							edge_State=1;
+    						}
     					break;
     					/*
 . Wait for DONE package
 . The counting process is DONE
     					*/
     					case 1:
     					edge_State=1;
+    				}
     			break;
     			/*
 . Wait for DONE package
 . The counting process is DONE
     			*/
     			case 1:
     						orb_set_color(YELLOW);
     						orb2_set_color(PURPLE);
     				orb_set_color(YELLOW);
     				orb2_set_color(PURPLE);
     						send2(CIRCLE_ACTION_ACK,robotid);		// keep sending the packet until we get a response
     				// keep sending the packet until we get a
     				// response
     				send2(CIRCLE_ACTION_ACK,robotid);
     						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)
+    						{
     							edge_State=2;
+    						}
     					break;
     				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)
+    				{
     					edge_State=2;
+    				}
     			break;
     					case 2:	// wait for the second, general, done packet.
     			// wait for the second, general, done packet.
     			case 2:
     						orb_set_color(YELLOW);
     						packet_data=wl_basic_do_default(&data_length);
     						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1] == centerid)
+    						{
     							state = 1;
+    						}
     					break;
     				orb_set_color(YELLOW);
     				packet_data=wl_basic_do_default(&data_length);
     				if(packet_data != 0 && data_length>=2 &&
     				   packet_data[0]==CIRCLE_ACTION_DONE &&
     				   packet_data[1] == centerid)
+    				{
     					state = 1;
+    				}
     			break;
+    			}
     			/* The CIRCLEUP command for EDGE */
     		break;
     			case 11:
     		/* The CIRCLEUP command for EDGE */
     				switch(edge_State)
+    				{
     		case 11:
     			switch(edge_State)
+    			{
     					case 0:
     						// COLOR afer DONE ---> MAGENTA
     						orb_set_color(MAGENTA);
     						faceFront();			// turn to face the beacon
     						forward(175);
     						//range_init();
     			case 0:
     				// COLOR afer DONE ---> MAGENTA
     				orb_set_color(MAGENTA);
     				// turn to face the beacon
     				faceFront();
     				forward(175);
     				//range_init();
     						distance = get_distance();
     				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);
     					distance = get_distance();
     					delay_ms(14);
     					time+=14;
     					if(time>50)
+    					{
     						faceFront();
     						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);
     							distance = get_distance();
     							delay_ms(14);
     							time+=14;
     							if(time>50)
+    							{
     								faceFront;
     								time=0;
+    							}
+    						}
+    					}
+    				}
     						stop();
     						orb_set_color(GREEN);
     				stop();
     				orb_set_color(GREEN);
     						send2(CIRCLE_ACTION_ACK, robotid);
     				send2(CIRCLE_ACTION_ACK, robotid);
     						stop();
     						state = 1;
     					break;
     				stop();
     				state = 1;
     			break;
+    			}
+    				}
     			break;
     		break;
     			/* An ORIENT series of steps for the EDGE robot. */
     		/* An ORIENT series of steps for the EDGE robot. */
     			case 12:
     		case 12:
     			switch(edge_State)
+    			{
     				switch(edge_State)
     			// waits for a packet to tell it to turn on the bom.
     			case 0:
     				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)
+    				{
     					bom_on();
     					orb_set_color(ORANGE);
     					send2(CIRCLE_ACTION_ACK,centerid);
     					edge_State = 1;
+    				}
     			break;
     					// waits for a packet to tell it to turn on the bom.
     					case 0:
     						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)
+    						{
     							bom_on();
     							orb_set_color(ORANGE);
     							send2(CIRCLE_ACTION_ACK,centerid);
     							edge_State = 1;
+    						}
     					break;
     			// waits for a packet to tell it that it has been
     			// received.
     			case 1:
     				orb2_set_color(YELLOW);
     				packet_data=wl_basic_do_default(&data_length);
     				if(packet_data != 0 && data_length==3 &&
     				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&
     				   packet_data[1] == robotid)
+    				{
     					bom_off();
     					direction = packet_data[2];
     					orb_set_color(YELLOW);
     					edge_State = 2;
+    				}
     			break;
     					// waits for a packet to tell it that it has been received.
     					case 1:
     						orb2_set_color(YELLOW);
     						packet_data=wl_basic_do_default(&data_length);
     						if(packet_data != 0 && data_length==3 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid)
+    						{
     							bom_off();
     							direction = packet_data[2];
     							orb_set_color(YELLOW);
     							edge_State = 2;
+    						}
     					break;
     			/*
     			 Wait for the center bot to send a DONE packet; then
     			 turn to face the right direction.
     			*/
     			case 2:
     				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 = 3;
+    				}
     			break;
     					/* Wait for the center bot to send a DONE packet; then turn to face the right direction. */
     					case 2:
     						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 = 3;
+    						}
     					break;
     			/* Turn until we reach the right direction */
     			case 3:
     				aboutFace(direction);
     				stop();
     				orb_set_color(YELLOW);
     				send2(CIRCLE_ACTION_DONE,robotid);
     				state = 1;
     			break;
     					/* Turn until we reach the right direction (DIRECTION) */
     					case 3:
     						aboutFace(direction);
     					break;
+    			}
+    				}
     		break;
     			break;
     		/* The DRIVE steps for the EDGE robot */
     		case 13:
     			/* The MOVE steps for the EDGE robot */
     			switch(edge_State)
+    			{
     			/* Wait for the command to move forward. */
     			case 0:
     				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(packet_data[1]*10);
     					delay_ms(packet_data[2]*1000);
     					stop();
     					state = 1;
+    				}
     			break;
     			case 13:
     				switch(edge_State)
+    				{
     					/* Wait for the command to move forward. */
     					case 0:
     						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(packet_data[1]*10);
     							delay_ms(packet_data[2]*1000);
     							edge_State = 1;
+    						}
     					break;
+    			}
     					/* Terminal. */
     					case 1:
     						stop();
     						state = 1;
     					break;
     		break;
     		/* The TURNL steps for the EDGE robot */
     		case 14:
     			orb_set_color(RED);
     				}	// end the EdgeState switch
     			/* Wait for specifications for the turn. */
     			break;		// break the Edge state in the main switch loop
     			// END for EDGE robots
     			packet_data=wl_basic_do_default(&data_length);
     			if(packet_data != 0 && data_length>=3 &&
     			   packet_data[0]==CIRCLE_ACTION_TURN)
+    			{
     				orb_set_color(BLUE);
     				left(packet_data[1]*10);
     				delay_ms(packet_data[2]*1000);
     				stop();
     				state = 1;
+    			}
     		break;
     		/* The TURNR steps for the EDGE robot */
     		case 15:
     			orb_set_color(RED);
     			/* Wait for specifications for the turn. */
     			packet_data=wl_basic_do_default(&data_length);
     			if(packet_data != 0 && data_length>=3 &&
     			   packet_data[0]==CIRCLE_ACTION_TURN)
+    			{
     				orb_set_color(BLUE);
     //***********************************************************************************************************************************************************************************
     				right(packet_data[1]*10);
     				delay_ms(packet_data[2]*1000);
     				stop();
     				state = 1;
+    			}
     		break;
     	// END for EDGE robots
     //******************************************************************************
     //******************************************************************************
     			/*
     			   The MACHINE for the BEACON state
     			*/
     		/*
     		   The MACHINE for the BEACON state
     		*/
     			/* the COUNT code for the BEACON */
     			case 20:
     				switch(beacon_State)
+    				{
     		/* the COUNT code for the BEACON */
     		case 20:
     			switch(beacon_State)
+    			{
     					/* 0. center  robots on wait for pressing button 1 */
     					case 0:
     						bom_on();
     						orb_set_color(BLUE);
     						robotsReceived = 0;
     						beacon_State=1;
     					break;
     			/* 0. center  robots on wait for pressing button 1 */
     			case 0:
     				bom_on();
     				orb_set_color(BLUE);
     				robotsReceived = 0;
     				beacon_State=1;
     			break;
     					/* 1. Send EXIST package to EDGE robots */
     					case 1:
     						orb_set_color(RED);
     						send2(CIRCLE_ACTION_EXIST,robotid);
     						beacon_State=2;
     					break;
     			/* 1. Send EXIST package to EDGE robots */
     			case 1:
     				orb_set_color(RED);
     				send2(CIRCLE_ACTION_EXIST,robotid);
     				beacon_State=2;
     			break;
     					/* 2. Count the number of the EDGE robots *******NOTE: at most  1000  times of loop ******  */
     					case 2:
     						waitingCounter++;
     						orb1_set_color(YELLOW);
     						orb2_set_color(BLUE);
     						packet_data=wl_basic_do_default(&data_length);
     			/* 2. Count the number of the EDGE robots
     			 *******NOTE: at most  1000  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(packet_data!=0 && data_length>=2 &&
     				   packet_data[0]==CIRCLE_ACTION_ACK)
+    				{
     					orb_set_color(RED);
     					orb2_set_color(BLUE);
     					// only add to list seen if you haven't
     					// gotten an ACK from this robot
     					if(used[packet_data[1]]==0)
+    					{
     						robotsReceived++;
     						used[packet_data[1]] = 1;
     								usb_puts("Added: ");
     								usb_puti(packet_data[1]);
     								usb_puts("\r\n");
+    							}
     						usb_puts("Added: ");
     						usb_puti(packet_data[1]);
     						usb_puts("\r\n");
+    					}
     							// NEW: sends a packet to each robot it receives telling them to be done.
     							send2(CIRCLE_ACTION_GOTYOU,packet_data[1]);
+    						}
     						if(waitingCounter >= 300){
     							beacon_State=3;
+    						}
     					break;
     					/* COUNTing is DONE.  Sending DONE package. */
     					case 3:
     						blink(robotsReceived);
     						orb_set_color(GREEN);
     						send2(CIRCLE_ACTION_DONE, robotid);
     						state = 2;
     					break;
     					// NEW: sends a packet to each robot it
     					// receives telling them to be done.
     					send2(CIRCLE_ACTION_GOTYOU,
     						packet_data[1]);
+    				}
     				if(waitingCounter >= 300)
+    				{
     					beacon_State=3;
+    				}
     			break;
     			/* COUNTing is DONE.  Sending DONE package. */
     			case 3:
     				blink(robotsReceived);
     				orb_set_color(GREEN);
     				send2(CIRCLE_ACTION_DONE, robotid);
     				state = 2;
     			break;
+    			}
     			/* The CIRCLEUP method for BEACON */
     			case 21:
     		break;
     				switch(beacon_State)
     		/* The CIRCLEUP method for BEACON */
     		case 21:
     			switch(beacon_State)
+    			{
     			/* Wait for all the robots to get to right distance */
     			case 0:
     				left(170);
     				orb1_set_color(YELLOW);
     				orb2_set_color(WHITE);
     				numOk = 0;
     				while(numOk<robotsReceived)
+    				{
     					/* Wait for all the robots to get to right distance/position */
     					case 0:
     						left(170);
     						orb1_set_color(YELLOW);
     						orb2_set_color(WHITE);
     					packet_data=
     					   wl_basic_do_default(&data_length);
     					if(packet_data!=0 && data_length>=2 &&
     					   packet_data[0]==CIRCLE_ACTION_ACK)
+    					{
     						numOk++;
+    					}
+    				}
     						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++;
+    							}
+    						}
     						state = 2;
     					break;
+    				}
     				state = 2;
     			break;
+    			}
     		break;
     			/* The ORIENT code for the beacon */
     			case 22:
     				switch(beacon_State)
     		/* The ORIENT code for the beacon */
     		case 22:
     			switch(beacon_State)
+    			{
     			/* Turns all the robots in the same direction */
     			case 0:
     				stop();
     				bom_off();
     				orb_set_color(ORANGE);
     				// for each robot, tells them to turn their bom
     				// on, then tells them which way to face.
     				for(int i=0; i < 17; i++)
+    				{
     					/* Turns all the robots in the same direction */
     					case 0:
     						stop();
     						bom_off();
     				    if(used[i] == 1)
+    				    {
     					send2(CIRCLE_ACTION_GOTYOU, i);
     					// waits for a response so it knows the
     					// BOM is on.
     					while(1)
+    					{
     					    orb_set_color(RED);
     					    orb2_set_color(WHITE);
     					    packet_data=wl_basic_do_default(
     						&data_length);
     					    if(packet_data!=0 && data_length>=2
     						&& packet_data[0]==
     						CIRCLE_ACTION_ACK)
+    					    {
     						orb_set_color(ORANGE);
     						// for each robot, tells them to turn their bom on, then tells them which way they should face.
     						for(int i=0; i < 17; i++)
+    						{
     							if(used[i] == 1)
+    							{
     								send2(CIRCLE_ACTION_GOTYOU, i);
     								while(1)	// waits for a response so it knows the BOM is on.
+    								{
     									orb_set_color(RED);
     									orb2_set_color(WHITE);
     									packet_data=wl_basic_do_default(&data_length);
     									if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
+    									{
     										orb_set_color(ORANGE);
     										break;
+    									}
+    								}
     						break;
+    					    }
+    					}
     					delay_ms(20);
     					bom_refresh(BOM_ALL);
     					direction = bom_get_max();
     					direction += 8;
     					if(direction > 15) direction -= 16;
     								bom_refresh(BOM_ALL);
     								direction = bom_get_max();
     								direction += 8;
     								if(direction > 15) direction -= 16;
     								send3(CIRCLE_ACTION_GOTYOU, i, direction);
     								delay_ms(20);
+    							}
+    						}
     						beacon_State = 1;
     					break;
     					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 1:
     						send2(CIRCLE_ACTION_DONE,robotid);
     						bom_on();
     						state = 2;
     					break;
     					send3(CIRCLE_ACTION_GOTYOU, i,
     					     direction);
     					delay_ms(20);
+    				    }
+    				}
     				beacon_State = 1;
     			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 1:
     				send2(CIRCLE_ACTION_DONE,robotid);
     				bom_on();
     				beacon_State = 2;
     			break;
     			case 2:
     				numOk = 0;
     				while(numOk < robotsReceived)
+    				{
     					orb_set_color(ORANGE);
     					packet_data=wl_basic_do_default(
     					   &data_length);
     					if(packet_data!=0 && data_length>=2 &&
     					   packet_data[0]==CIRCLE_ACTION_DONE)
+    					{
     						numOk++;
+    					}
+    				}
     				state = 2;
     			break;
+    			}
     		break;
     			/* The DRIVE code for the beacon */
     			case 23:
     				switch(beacon_State)
+    				{
     		/* The DRIVE code for the beacon */
     		case 23:
     					/* Tells the robots to move forward and moves itself. */
     					case 0:
     						orb_set_color(YELLOW);
     						delay_ms(5000);
     			switch(beacon_State)
+    			{
     						// format: type of ack, speed divided by 10, time in seconds.
     						send3(CIRCLE_ACTION_FORWARD,20,2);
     						orb_set_color(BLUE);
     						forward(200);
     						delay_ms(2000);
     						stop();
     						beacon_State = 1;
     					break;
     			/* Tells the robots to move forward and moves itself. */
     			case 0:
     				orb_set_color(YELLOW);
     				delay_ms(50);
     					/* Terminal. */
     					case 1:
     						stop();
     						state = 2;
     					break;
+    				}
     				// format: type of ack, speed divided by 10,
     				// time in seconds.
     				send3(CIRCLE_ACTION_FORWARD,speed,duration);
     				orb_set_color(BLUE);
     				forward(speed*10);
     				delay_ms(duration*1000);
     				stop();
     				beacon_State = 1;
     			break;
     //***********************************************************************************************************************************************************************************
     			/* Terminal. */
     			case 1:
     				stop();
     				state = 2;
     			break;
+    			}
     		break;
     		/* The TURNL code for the beacon */
     		case 24:
     			orb_set_color(YELLOW);
     			delay_ms(50);
     			// format: type of ack, speed divided by 10,
     			// time in seconds.
     			for(int i = 0 ; i < 5; i++)
     				send3(CIRCLE_ACTION_TURN,speed,duration);
     			orb_set_color(BLUE);
     			left(speed*10);
     			delay_ms(duration*1000);
     			stop();
     			state = 2;
     		break;
     		/* The TURNR code for the beacon */
     		case 25:
     			orb_set_color(YELLOW);
     			delay_ms(50);
     			// format: type of ack, speed divided by 10,
     			// time in seconds.
     			for(int i = 0 ; i < 5; i++)
     				send3(CIRCLE_ACTION_TURN,speed,duration);
     			orb_set_color(BLUE);
     			right(speed*10);
     			delay_ms(duration*1000);
     			stop();
     			state = 2;
     		break;
     //******************************************************************************
     //******************************************************************************
     		}	// ends the main switch
     	}		// ends the main while loop
     	orb_set_color(RED);	// error, we should never break from the while loop!
     	// error, we should never break from the while loop!
     	orb_set_color(RED);
     	while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */
     	/*
     	 END HERE, just in case something happened.
     	 This way we can see the red orb.
     	*/
     	while(1);
+    }

trunk/code/behaviors/formation_control/circle/circle.h (revision 1807)
19	19	#define CIRCLE_ACTION_FORWARD 'F'
20	20	#define CIRCLE_CLAIM_CENTER 'C'
21	21	#define CIRCLE_EXECUTE 'X'
	22	#define CIRCLE_ACTION_TURN 'T'
22	23
23
24	24	#endif

Also available in: Unified diff