Revision 1930 trunk/code/projects/calibration_platform/robot/cal_sta_robot.c

View differences:

cal_sta_robot.c
1
/*
2
 * template.c - A starting point for developing behaviors using the Colony
3
 * 		robots. To create a new behavior, you should copy this "template"
4
 * 		folder to another folder and rename the "template.c" file
5
 * 		appropriately.
6
 *
7
 * This template will have the robot drive in circles and flash the orbs.
8
 *
9
 * Author: John Sexton, Colony Project, CMU Robotics Club
10
 */
1 11

  
2
/*** PROGRAM INFORMATION ***
3

  
4
   This program assembles a group of robots into a circle and allows the
5
movement within that formation.  Robots should be able to break formation and
6
travel as a line, readjust in the face of obstacles, and reform if conditions
7
are necessary.
8

  
9
   The program begins waiting for a button press.  When pressed, a robot assumes
10
the BEACON position, which means that it is the robot in the center of the
11
circle and therefore in charge.  It then gathers robots around it by sending
12
them commands.  This code is executed using two finite state machines, nested
13
inside one another.
14
   One controls the overall state of the robot (whether it is a BEACON, an EDGE,
15
or WAITING, for example).
16

  
17
   This code should be implemented so that most useful functions are built in
18
to the machine.  For example, the BEACON robot should be able to call methods
19
such as CircleUp() to gather robots around it, and Move(distance) to move the
20
circle group all at once.
21

  
22
   This Code is the property of the Carnegie Mellon Robotics Club and is being
23
used to test formation control in a low-cost robot colony.  Thanks to all
24
members of RoboClub, especially Colony president John Sexton and graduade
25
student representative Chris Mar.
26
	
27
   AUTHORS: James Carroll, Steve DeVincentis, Hanzhang (Echo) Hu, Nico Paris,
28
Joel Rey, Reva Street, Alex Zirbel
29
 	*/
30

  
31

  
32 12
#include <dragonfly_lib.h>
33 13
#include <wl_basic.h>
34
#include <encoders.h>
35
#include "cal_sta_robot.h"
36 14

  
37
/*** TODO: ***
15
/* Time delay which determines how long the robot circles before it
16
 * changes direction. */
17
#define TIME_DELAY 1000
38 18

  
39
   -Transform the code into a method-based state machine that uses the
40
procedural state machines, which are hardcoded and hard to edit, as a backup.
19
int main (void) {
41 20

  
42
   -Implement a drive straight method for use in keeping the robots more
43
accurate as a group.
44

  
45
   -Fix the approach method: good robots usually work well, but bad robots often
46
have errors which might be avoidable with the use of error checking.
47

  
48
   -Make robots more robust: packages are often lost, which throws the entire
49
procedural nature of the program off.
50

  
51
   -Consider using the center bot to check distances
52

  
53
   -More testing is always good and necessary.					
54
*/
55

  
56
/*** BOT LOG ***
57

  
58
   4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well.
59
   4-2-2010: BOT 7 and BOT 14 worked extremely well, no matter states.  BOT 1
60
       	started well, but malfunctioned later.
61
        */
62

  
63
/*** TERMINOLOGY ***
64

  
65
  WAITINGSTATE:
66
   The robot waits to be given a signal to do something.  Wireless is on,
67
in case the robot is called on to turn into an EDGE.  The color should be LIME
68
or YELLOW-GREEN.
69

  
70
  BEACON_CONTROL:
71
   The code that executes commands when a robot is turned to BEACON mode.  This
72
code may run predefined methods for simplicity.  One goal is to make these
73
methods change the robot turn to to BEACON_MACHINE mode for a while, and then
74
return to the CONTROL code where they left off.
75

  
76
  EDGE_CONTROL:
77
   Like BEACON_CONTROL, executes whatever orders are required of the robot as an
78
EDGE.
79

  
80
  BEACON_MACHINE:
81
   A hardcoded list of functions which the robot is capable of running through.
82
Consists of a finite state machine, where the robot executes a set of commands
83
in a procedural manner and then returns to wherever it was in the control code.
84

  
85
  EDGE_MACHINE:
86
   Like the BEACON_MACHINE, but contains the same sort of procedural information
87
for EDGE robots.
88

  
89
  END:
90
   A terminal state of the machine, where the robot just sits and waits.  The
91
color should be GREEN and WHITE.
92

  
93

  
94
  TYPES OF WIRELESS PACKETS:
95

  
96
   CIRCLE_ACTION_EXIST 'E'
97
   CIRCLE_ACTION_POSITION 'P'
98
   CIRCLE_ACTION_ACK 'A'
99
 	A general acknowledgement package.
100
   CIRCLE_ACTION_DONE 'D'
101
 	Used by robots to tell when they have finished their action.
102
   CIRCLE_ACTION_GOTYOU 'G'
103
 	Used by the BEACON to tell a robot when it has been checked off.
104
 	At this point, the EDGE has been recognized.  Used for times when
105
 	all EDGE robots have to communicate to the center via the spam method.
106
   CIRCLE_ACTION_FORWARD 'F'
107
 	The BEACON tells the rest of the robots to move forward.
108
   CIRCLE_CLAIM_CENTER 'C'
109
 	Sent out by a robot when it takes over as BEACON.
110
 	*/
111

  
112

  
113
/* Define some variables to keep track of the state machine.*/
114
int END = 100;	
115
int WAITINGSTATE = 0;
116
int EDGE_CONTROL = 1;
117
int BEACON_CONTROL = 2;
118
int EDGE_MACHINE = 3;
119
int BEACON_MACHINE = 4;
120

  
121
int COUNT = 0;
122
int CIRCLEUP = 1;
123
int ORIENT = 2;
124
int DRIVE = 3;
125
int TURNL = 4;
126
int TURNR = 5;
127

  
128
int currentPos = 0;
129
int state = 0;
130

  
131
// keep track of the speed and duration of group movements.
132
int speed = 20;
133
int duration = 2;
134

  
135
int timeout = 0;
136
int sending = 0;
137
int stop2 = 0;
138
struct vector slave_position;
139
int desired_max_bom;
140
int bom_max_counter;
141

  
142

  
143
void switch_sending(void)
144
{
145
	if(sending)
146
	{
147
		sending = 0;
148
		bom_off();
149
	}
150
	else
151
	{
152
		sending = 1;
153
		bom_on();
154
	}
155
}
156

  
157
// set the motors to this forward speed.
158
void forward(int speed)
159
{		
160
	motor_l_set(FORWARD,speed);
161
	motor_r_set(FORWARD,speed);
162
}
163
// turn left at this speed.
164
void left(int speed)
165
{	
166
	motor_l_set(BACKWARD,speed);
167
	motor_r_set(FORWARD,speed);
168
}
169
void right(int speed)
170
{
171
	motor_l_set(FORWARD,speed);
172
	motor_r_set(BACKWARD,speed);
173
}
174
// stop() is better than motors_off(), which creates a slight delay when
175
// reactivating the motors.  Stop() is faster.
176
void stop(void)
177
{
178
	motor_l_set(BACKWARD,0);
179
	motor_r_set(FORWARD,0);
180
}
181
void setforward(int spd1, int spd2)
182
{
183
	motor_l_set(FORWARD,spd1);
184
	motor_r_set(FORWARD,spd2);
185
}
186
void backward(int speed)
187
{
188
	motor_l_set(BACKWARD, speed);
189
	motor_r_set(BACKWARD, speed);
190
}
191
// takes an averaged reading of the front rangefinder
192
int get_distance(void)
193
{
194
	// kk sets this to 5 readings.
195
	int temp,distance,kk=5;			
196
	distance =0;
197
	for (int i=0; i<kk; i++)
198
	{
199
		temp = range_read_distance(IR2);
200
		if (temp == -1)
201
		{
202
			//temp=0;
203
			i--;
204
		}
205
		else
206
			distance+= temp;
207
		delay_ms(3);
208
	}
209
	if (kk>0)
210
		return (int)(distance/kk);
211
	else 
212
		return 0;
213
}
214

  
215
/* Sends a global packet with two arguments */
216
void send2(char arg0, char arg1)
217
{
218
	char send_buffer[2];
219
	send_buffer[0]=arg0;
220
	send_buffer[1]=arg1;
221
	wl_basic_send_global_packet(42,send_buffer,2);
222
}
223

  
224
/* Sends a global packet with three arguments */
225
void send3(char arg0, char arg1, char arg2)
226
{
227
	char send_buffer[3];
228
	send_buffer[0]=arg0;
229
	send_buffer[1]=arg1;
230
	send_buffer[2]=arg2;
231
	wl_basic_send_global_packet(42,send_buffer,3);
232
}
233

  
234
/*
235
   Orients the robot so that it is facing the beacon (or the broadcasting BOM).
236
*/
237
void faceFront(void)
238
{
239
	int counter = 0;
240
	int currentDir = 0;
241
	left(200);
242
	int bomNum = -1;
243
	orb1_set_color(BLUE);
244
	while(bomNum != 4)
245
	{
246
		if(counter >= 5)
247
		{
248
			forward(200);
249
			delay_ms(750);
250
			counter = 0;
251
		}
252
		bom_refresh(BOM_ALL);
253
		bomNum = bom_get_max();
254
		if(bomNum == -1)
255
		{
256
			//ignore
257
		}
258
		else if((bomNum < 4) || (bomNum >= 12))
259
		{
260
			right(200);
261
			if(currentDir == 0)
262
				counter++;
263
			currentDir = 1;
264
		}
265
		else
266
		{
267
			left(200);
268
			if(currentDir == 1)
269
				counter++;
270
			currentDir = 0;
271
		}
272
	}
273
	stop();
274
	return;
275
}
276

  
277
/*
278
   Turns the robot slowly to the right until it reaches the BOM reading goal.
279
   More stable code than what was implemented ealier, with smart turning,
280
   but slower.
281
*/
282
void aboutFace(int goal)
283
{
284
	int bomNum = -1;
285
	int speed = 170;	// speed with which to turn
286

  
287
	orb1_set_color(BLUE);	// BLUE and PURPLE
21
	/* Initialize the dragonfly boards, the xbee, and the encoders */
22
	dragonfly_init(ALL_ON);
23
	xbee_init();
24
	encoders_init();
288 25
	
289
	while(bomNum != goal)
290
	{
291
		// bomNum is the current maximum reading
292
		bom_refresh(BOM_ALL);
293
		bomNum = bom_get_max();
294
		right(speed);
295
	}
296
	stop();
297
	return;
298
}
26
	while (1) {
27
		/* Drive left, set orbs, and wait */
28
		orbs_set_color(RED, GREEN);
29
        usb_puts("Robot\n");
30
        delay_ms(TIME_DELAY);
299 31

  
300

  
301
/* 
302
   BLINK the given number times
303
*/
304
void blink(int num)
305
{
306
	for(int i = 0; i<num; i++)
307
	{
308
		orb_set_color(ORB_OFF);
309
		delay_ms(150);
310
		orb_set_color(RED);
311
		delay_ms(50);
32
		/* Drive right, change orb colors, and wait */
33
		orbs_set_color(PURPLE, BLUE);
34
        usb_puts("Marvin\n");
35
		delay_ms(TIME_DELAY);
312 36
	}
313
	orb_set_color(ORB_OFF);
314
}
315 37

  
316
/* 
317
   BLINK slowly the given number times
318
*/
319
void slowblink(int num)
320
{
321
	for(int i = 0; i<num; i++)
322
	{
323
		orb_set_color(ORB_OFF);
324
		delay_ms(300);
325
		orb_set_color(RED);
326
		delay_ms(200);
327
	}
328
	orb_set_color(ORB_OFF);
329 38
}
330

  
331
/*
332
   A method for the higher-level code for the BEACON.  The beacon can make
333
   any of the preprogrammed commands, and this code sends the packet and
334
   transitions the robots correctly.
335
*/
336
void order(int action)
337
{
338
	currentPos++;
339
	send2(CIRCLE_EXECUTE, action);
340
	state = 20 + action;
341
}
342

  
343
/*
344
   A method for the higher-level code for the BEACON.  The beacond sends
345
   not only the command, but also the speed and duration for which the
346
   (movement) command is to be executed.
347
*/
348
void orderMove(int action, int newSpeed, int newDuration)
349
{
350
	currentPos++;
351
	speed = newSpeed;
352
	duration = newDuration;
353
	send2(CIRCLE_EXECUTE, action);
354
	state = 20 + action;
355
}
356

  
357
/*
358
   Turns off the motors, sends an EXECUTE packet, and blinks green and white
359
   forever.
360
*/
361
void terminate(void)
362
{
363
	motors_off();
364
	send2(CIRCLE_EXECUTE, 100);
365
	orb_set_color(GREEN);
366
	orb2_set_color(WHITE);
367
	while(1) ;
368
}
369

  
370

  
371
//******************************************************************************
372
//******************************************************************************
373
//******************************************************************************
374

  
375

  
376

  
377
/*
378
   A state machine with five states.  The robot starts out in WAITINGSTATE mode,
379
from which it recieves a signal of some sort and moves to a different state.
380
*/
381
int main(void)
382
{
383
	/* Initialize dragonfly board */
384
    	dragonfly_init(ALL_ON);
385
    	/* Initialize the basic wireless library */
386
    	wl_basic_init_default();
387
    	/* Set the XBee channel to 24 - must be standard among robots */
388
	wl_set_channel(24);
389

  
390
	int robotid = get_robotid();
391

  
392
	// once the EDGE gets the first signal from a center, it stores who the 	// center is.
393
	int centerid = 0;
394

  
395
	// stores a list of bots which are in the group by storing a "1" in the
396
	// array if the robot of that index is in the group.	
397
	int used[17];
398
	int numOk;
399

  
400
	// initially, no robots in the group.
401
	for (int i=0; i<17; i++)
402
		used[i] = 0;
403

  
404
	// keeps track of the length of wireless packets received.
405
	int data_length;
406
	unsigned char *packet_data=wl_basic_do_default(&data_length);
407

  
408
	// these variables keep track of the inner state machines in the
409
	//  procedural MACHINE states.	
410
	int beacon_State=0;
411
	int edge_State=0;
412

  
413
	int waitingCounter=0;
414
	
415
	// an important variable that stores the size of the group.
416
	int robotsReceived=0;
417
	
418
	// offset for the approaching: how far off the rangefinders can be
419
	int offset = 20;
420
	int time=0;
421

  
422
	// keeps track of which way robots are facing relative to the center
423
	int direction = 4;
424

  
425
	// how far away the robot is.  Initialized to a large value to ensure
426
	// that the robot doesn't think it is already the right distance away.
427
	int distance=1000;
428
	int onefoot = 250;	// how far away to stop.
429
	
430
	while(1)
431
	{
432
		bom_refresh(BOM_ALL);
433

  
434
	/***EXPECTED MOVES***
435
	   (OUT OF DATE.  Will be updated once changes have been made.)
436
	The designed movement:
437
	 1. one center robot, several edge robots are on;
438
	 2. center robots: button 1 is pressed;
439
	 3. center robots: send global package telling edges that he exists;
440
	 4. EDGE robots response with ACK. 
441
	 5. EDGE robots wait for center robots to finish counting (DONE package)
442
	 6. EDGE robtos approach the center robtot and stop at the "onefoot"
443
		 distance, send message to the center
444
		*/
445
		
446
		
447
		/*
448
		  This is the MAIN SWITCH LOOP, which governs the overall
449
		  status of the robot.
450
		 */
451
		switch(state)
452
		{
453

  
454

  
455
		/*
456
		The WAITINGSTATE.  This state constantly checks for wireless
457
		packets,
458
		and updates its state as soon as it receives a signal.
459
		*/
460
		case 0:
461

  
462
			orb_set_color(YELLOW);
463
			packet_data=wl_basic_do_default(&data_length);
464
			if(packet_data != 0 && data_length>=2
465
				&& packet_data[0]==CIRCLE_CLAIM_CENTER)
466
			{
467
				centerid = packet_data[1];
468
				state = 1;
469
			}
470

  
471
			if(button1_read())
472
			{
473
				// becomes the center if button1 is clicked.
474
				send2(CIRCLE_CLAIM_CENTER, robotid); 	
475
				state = 2;
476
			}
477
		break;
478

  
479

  
480

  
481
//******************************************************************************
482
//******************************************************************************
483

  
484

  
485
		/*
486
		The CONTROL for the EDGE state.  This sets a certain procedure
487
		to follow, in the form of simple
488
		commands, for a robot to follow if it is set to an EDGE.
489
		*/
490

  
491
		case 1:
492
			orb_set_color(CYAN);
493
			orb1_set_color(YELLOW);
494

  
495
			int command = -1;
496
			
497
			packet_data=wl_basic_do_default(&data_length);
498

  
499
			if(packet_data != 0 && data_length>=2 &&
500
			   packet_data[0]==CIRCLE_EXECUTE)
501
			{
502
				command = packet_data[1];
503
			}
504

  
505
			if(command != -1)
506
			{
507
				edge_State = 0;
508
				switch(command)
509
				{
510
				case 0:
511
					state = 10; break;
512

  
513
				case 1:
514
					state = 11; break;
515

  
516
				case 2:
517
					state = 12; break;
518

  
519
				case 3:
520
					state = 13; break;
521

  
522
				case 4:
523
					state = 14; break;
524

  
525
				case 5:
526
					state = 15; break;
527

  
528
				case 100:
529
					terminate(); break;
530
				}
531
			}
532

  
533
		break;
534

  
535

  
536

  
537
//******************************************************************************
538
//******************************************************************************
539

  
540

  
541
		/*
542
		The CONTROL for the BEACON state.  This sets a certain procedure
543
		to follow, in the form of simple commands, for a robot to follow
544
		if it is set to a BEACON.
545
		*/
546
		case 2:
547
			orb_set_color(PURPLE);
548
			beacon_State = 0;
549

  
550
			switch(currentPos)
551
			{
552
			case 0:
553
				order(COUNT);	break;
554

  
555
			case 1:
556
				order(CIRCLEUP); break;
557

  
558
			case 2:
559
				order(ORIENT); break;
560

  
561
			case 3:
562
				orderMove(DRIVE,20,2); break;
563

  
564
			case 4:
565
				order(CIRCLEUP); break;
566

  
567
			case 5:
568
				order(ORIENT); break;
569

  
570
			case 6:
571
				orderMove(TURNR,18,1); break;
572

  
573
			case 7:
574
				orderMove(DRIVE,20,2); break;
575

  
576
			case 8:
577
				order(CIRCLEUP); break;
578

  
579
			case 9:
580
				order(ORIENT); break;
581

  
582
			case 10:
583
				terminate(); break;
584

  
585
			}
586
				
587
		break;
588

  
589

  
590
//******************************************************************************
591
//******************************************************************************
592

  
593

  
594
	/* The following states are MACHINE states for the EDGE robot. */
595

  
596
		/*
597
			EDGE on COUNT
598
		*/
599
		case 10:	
600

  
601
			switch(edge_State)
602
			{
603
				/*
604
				0. EDGE robots are on. 
605
				1. They are waiting for EXIST pacakage from the
606
					Center robots
607
				2. After they receive the package, they send ACK
608
					package to center.
609
				3. Done for now: display green.
610
				*/
611
			case 0:
612
				bom_off();
613
				orb1_set_color(YELLOW);
614
				orb2_set_color(BLUE);
615
				packet_data=wl_basic_do_default(&data_length);
616

  
617
				if(packet_data != 0 && data_length>=2 &&
618
				   packet_data[0]==CIRCLE_ACTION_EXIST)
619
				{
620
					centerid = packet_data[1];
621

  
622
					send2(CIRCLE_ACTION_ACK,robotid);
623

  
624
					edge_State=1;
625
				}
626
			break;
627

  
628
			/*
629
				1. Wait for DONE package 
630
				2. The counting process is DONE
631
			*/
632
			case 1:		
633
					
634
				orb_set_color(YELLOW);
635
				orb2_set_color(PURPLE);
636

  
637
				// keep sending the packet until we get a
638
				// response
639
				send2(CIRCLE_ACTION_ACK,robotid);
640
						
641
				packet_data=wl_basic_do_default(&data_length);
642
				if(packet_data != 0 && data_length>=2 &&
643
				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&
644
				   packet_data[1] == robotid)
645
				{
646
					edge_State=2;
647
				}
648
			break;
649

  
650
			// wait for the second, general, done packet.
651
			case 2:	
652

  
653
				orb_set_color(YELLOW);
654
				packet_data=wl_basic_do_default(&data_length);
655
				if(packet_data != 0 && data_length>=2 &&
656
				   packet_data[0]==CIRCLE_ACTION_DONE &&
657
				   packet_data[1] == centerid)
658
				{
659
					state = 1;
660
				}
661
			break;
662
			}
663

  
664
		break;
665

  
666
		/* The CIRCLEUP command for EDGE */
667

  
668
		case 11:
669

  
670
			switch(edge_State)
671
			{
672
					
673
			case 0:
674
				// COLOR afer DONE ---> MAGENTA
675
				orb_set_color(MAGENTA);
676
				// turn to face the beacon
677
				faceFront();
678
				forward(175);
679
				//range_init();
680
						
681
						
682
				distance = get_distance();
683
				time=0;
684
				while ((distance-offset)>=onefoot ||
685
				   distance==0 || (distance+offset)<onefoot)
686
				{
687
					if(distance==0)
688
						orb_set_color(WHITE);
689
					else if(distance-offset>=onefoot)
690
						forward(175);
691
					else
692
						backward(175);
693
					distance = get_distance();
694
					delay_ms(14);
695
					time+=14;
696
					if(time>30)
697
					{
698
						faceFront();
699
						time=0;
700
					}
701
				}
702
							
703
				stop();
704
				orb_set_color(GREEN);
705

  
706
				send2(CIRCLE_ACTION_ACK, robotid);
707

  
708
				stop();
709
				state = 1;
710
			break;
711
			}
712

  
713

  
714
		break;
715
		
716
		/* An ORIENT series of steps for the EDGE robot. */
717

  
718
		case 12:
719
			
720
			switch(edge_State)
721
			{
722

  
723
			// waits for a packet to tell it to turn on the bom.
724
			case 0:
725
				packet_data=wl_basic_do_default(&data_length);
726
				if(packet_data != 0 && data_length==2 &&
727
				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&
728
				   packet_data[1] == robotid)
729
				{
730
					bom_on();
731
					orb_set_color(ORANGE);
732
					send2(CIRCLE_ACTION_ACK,centerid);
733
					edge_State = 1;
734
				}
735
			break;
736

  
737
			// waits for a packet to tell it that it has been
738
			// received.
739
			case 1:
740
				orb2_set_color(YELLOW);
741
				packet_data=wl_basic_do_default(&data_length);
742
				if(packet_data != 0 && data_length==3 &&
743
				   packet_data[0]==CIRCLE_ACTION_GOTYOU &&
744
				   packet_data[1] == robotid)
745
				{
746
					bom_off();
747
					direction = packet_data[2];
748
					orb_set_color(YELLOW);
749
					edge_State = 2;
750
				}
751
			break;
752

  
753
			/*
754
			 Wait for the center bot to send a DONE packet; then
755
			 turn to face the right direction.
756
			*/
757
			case 2:
758
				orb_set_color(GREEN);
759
				packet_data=wl_basic_do_default(&data_length);
760
				if(packet_data != 0 && data_length>=2 &&
761
				   packet_data[0]==CIRCLE_ACTION_DONE)
762
				{
763
					orb_set_color(WHITE);
764
					orb2_set_color(CYAN);
765
					edge_State = 3;
766
				}
767
			break;
768

  
769
			/* Turn until we reach the right direction */
770
			case 3:
771
				aboutFace(direction);
772
				stop();
773
				orb_set_color(YELLOW);
774
				send2(CIRCLE_ACTION_DONE,robotid);
775
				state = 1;
776
			break;
777

  
778
			}
779

  
780
		break;
781

  
782

  
783
		/* The DRIVE steps for the EDGE robot */
784
		case 13:
785
			
786
			/* Wait for specifications to drive */
787
			packet_data=wl_basic_do_default(&data_length);
788
			if(packet_data != 0 && data_length>=3 &&
789
			   packet_data[0]==CIRCLE_ACTION_FORWARD)
790
			{
791
				orb_set_color(BLUE);
792

  
793
				forward(packet_data[1]*10);
794
				delay_ms(packet_data[2]*1000);
795
				stop();
796
				state = 1;
797
			}
798

  
799
		break;
800

  
801
		/* The TURNL steps for the EDGE robot */
802
		case 14:
803

  
804
			/* Wait for specifications for the turn. */
805
			packet_data=wl_basic_do_default(&data_length);
806
			if(packet_data != 0 && data_length>=3 &&
807
			   packet_data[0]==CIRCLE_ACTION_TURN)
808
			{
809
				orb_set_color(BLUE);
810

  
811
				left(packet_data[1]*10);
812
				delay_ms(packet_data[2]*1000);
813
				stop();
814
				state = 1;
815
			}
816
		break;
817

  
818
		/* The TURNR steps for the EDGE robot */
819
		case 15:
820

  
821
			/* Wait for specifications for the turn. */
822
			packet_data=wl_basic_do_default(&data_length);
823
			if(packet_data != 0 && data_length>=3 &&
824
			   packet_data[0]==CIRCLE_ACTION_TURN)
825
			{
826
				orb_set_color(BLUE);
827

  
828
				right(packet_data[1]*10);
829
				delay_ms(packet_data[2]*1000);
830
				stop();
831
				state = 1;
832
			}
833
		break;
834
				
835
	// END for EDGE robots
836
			
837

  
838

  
839
//******************************************************************************
840
//******************************************************************************
841

  
842

  
843
		/*
844
		   The MACHINE for the BEACON state
845
		*/
846

  
847
		/* the COUNT code for the BEACON */
848
		case 20:
849
			switch(beacon_State) 
850
			{
851

  
852
			/* 0. center  robots on wait for pressing button 1 */
853
			case 0:
854
				bom_on();
855
				orb_set_color(BLUE);
856
				robotsReceived = 0;
857
				beacon_State=1;
858
			break;	
859

  
860
			/* 1. Send EXIST package to EDGE robots */
861
			case 1:
862
				orb_set_color(RED);
863
				send2(CIRCLE_ACTION_EXIST,robotid);
864
				beacon_State=2;
865
			break;
866

  
867
			/* 2. Count the number of the EDGE robots
868
			 *******NOTE: at most  1000  times of loop ******  */
869
			case 2:
870
				waitingCounter++;
871
				orb1_set_color(YELLOW);
872
				orb2_set_color(BLUE);
873
				packet_data=wl_basic_do_default(&data_length);
874
					
875
				if(packet_data!=0 && data_length>=2 &&
876
				   packet_data[0]==CIRCLE_ACTION_ACK)
877
				{
878
					orb_set_color(RED);
879
					orb2_set_color(BLUE);
880
					// only add to list seen if you haven't
881
					// gotten an ACK from this robot
882
					if(used[packet_data[1]]==0)
883
					{
884
						robotsReceived++;
885
						used[packet_data[1]] = 1;
886

  
887
						usb_puts("Added: ");
888
						usb_puti(packet_data[1]);
889
						usb_puts("\r\n");
890
					}
891

  
892
					// NEW: sends a packet to each robot it
893
					// receives telling them to be done.
894
					send2(CIRCLE_ACTION_GOTYOU,
895
						packet_data[1]);
896
				}
897
				if(waitingCounter >= 300)
898
				{
899
					beacon_State=3;
900
				}
901
			break;
902

  
903
			/* COUNTing is DONE.  Sending DONE package. */	
904
			case 3:
905
				blink(robotsReceived);
906
				orb_set_color(GREEN);
907
				send2(CIRCLE_ACTION_DONE, robotid);
908
				state = 2;
909
			break;
910
			}
911

  
912
		break;
913

  
914
		/* The CIRCLEUP method for BEACON */
915
		case 21:
916

  
917
			switch(beacon_State)
918
			{
919

  
920
			/* Wait for all the robots to get to right distance */
921
			case 0: 
922
		//		left(170);
923
				orb1_set_color(YELLOW);
924
				orb2_set_color(WHITE);
925
			
926
				numOk = 0;
927
			
928
				while(numOk<robotsReceived)
929
				{
930
					packet_data=
931
					   wl_basic_do_default(&data_length);
932
					if(packet_data!=0 && data_length>=2 &&
933
					   packet_data[0]==CIRCLE_ACTION_ACK)
934
					{
935
						numOk++;
936
					}
937
				}
938
					
939
				state = 2;
940
			break;
941
			}
942

  
943
		break;
944

  
945

  
946
		/* The ORIENT code for the beacon */
947
		case 22:
948

  
949
			switch(beacon_State)
950
			{
951
			/* Turns all the robots in the same direction */
952
			case 0:
953
				stop();
954
				bom_off();
955
				orb_set_color(ORANGE);
956
						
957
				// for each robot, tells them to turn their bom
958
				// on, then tells them which way to face.
959
				for(int i=0; i < 17; i++)
960
				{
961
				    if(used[i] == 1)
962
				    {
963
					send2(CIRCLE_ACTION_GOTYOU, i);
964
					// waits for a response so it knows the
965
					// BOM is on.
966
					while(1)
967
					{
968
					    orb_set_color(RED);
969
					    orb2_set_color(WHITE);
970
					    packet_data=wl_basic_do_default(
971
						&data_length);
972
					    if(packet_data!=0 && data_length>=2
973
						&& packet_data[0]==
974
						CIRCLE_ACTION_ACK)
975
					    {
976
						orb_set_color(ORANGE);
977
						break;
978
					    }
979
					}
980
					delay_ms(20);
981
					bom_refresh(BOM_ALL);
982
					direction = bom_get_max();
983
				
984
					direction += 8;
985
					if(direction > 15) direction -= 16;
986

  
987
					delay_ms(20);
988
			
989
					send3(CIRCLE_ACTION_GOTYOU, i,
990
					     direction);
991
		
992
					delay_ms(20);
993
				    }
994
				}
995
				beacon_State = 1;
996
			break;
997
			
998
			/*
999
			Sends a DONE packet to signify that it has read in all
1000
			the robots' directions and sent packets.
1001
			Edge robots should now turn to face the right direction.
1002
			*/
1003
			case 1:
1004
				send2(CIRCLE_ACTION_DONE,robotid);
1005
				bom_on();
1006
				beacon_State = 2;
1007
			break;
1008
			
1009
			case 2:
1010
				numOk = 0;
1011

  
1012
				while(numOk < robotsReceived)
1013
				{
1014
					orb_set_color(ORANGE);
1015
					packet_data=wl_basic_do_default(
1016
					   &data_length);
1017

  
1018
					if(packet_data!=0 && data_length>=2 &&
1019
					   packet_data[0]==CIRCLE_ACTION_DONE)
1020
					{
1021
						numOk++;
1022
					}
1023
				}
1024
				state = 2;
1025
			break;
1026
			}
1027

  
1028
		break;
1029

  
1030

  
1031
		/* The DRIVE code for the beacon */
1032
		case 23:
1033

  
1034
			orb_set_color(YELLOW);
1035
			delay_ms(100);
1036

  
1037
			// format: type of ack, speed divided by 10,
1038
			// time in seconds.
1039
			for(int i = 0 ; i < 13; i++)
1040
				send3(CIRCLE_ACTION_FORWARD,speed,duration);
1041
			orb_set_color(BLUE);
1042
			forward(speed*10);
1043
			delay_ms(duration*1000);
1044
			stop();
1045
			state = 2;
1046
		break;
1047

  
1048
		/* The TURNL code for the beacon */
1049
		case 24:
1050

  
1051
			orb_set_color(YELLOW);
1052
			delay_ms(100);
1053

  
1054
			// format: type of ack, speed divided by 10,
1055
			// time in seconds.
1056
			for(int i = 0 ; i < 13; i++)
1057
				send3(CIRCLE_ACTION_TURN,speed,duration);
1058
			orb_set_color(BLUE);
1059
			left(speed*10);
1060
			delay_ms(duration*1000);
1061
			stop();
1062
			state = 2;
1063

  
1064
		break;
1065

  
1066
		/* The TURNR code for the beacon */
1067
		case 25:
1068

  
1069
			orb_set_color(YELLOW);
1070
			delay_ms(100);
1071

  
1072
			// format: type of ack, speed divided by 10,
1073
			// time in seconds.
1074
			for(int i = 0 ; i < 13; i++)
1075
				send3(CIRCLE_ACTION_TURN,speed,duration);
1076
			orb_set_color(BLUE);
1077
			right(speed*10);
1078
			delay_ms(duration*1000);
1079
			stop();
1080
			state = 2;
1081

  
1082
		break;
1083

  
1084

  
1085
//******************************************************************************
1086
//******************************************************************************
1087
		
1088
		}	// ends the main switch
1089
	}		// ends the main while loop
1090

  
1091
	// error, we should never break from the while loop!
1092
	orb_set_color(RED);	
1093

  
1094
	/*
1095
	 END HERE, just in case something happened.
1096
	 This way we can see the red orb.
1097
	*/
1098
	while(1); 
1099
}

Also available in: Unified diff