Statistics
| Revision:

root / trunk / code / projects / calibration_platform / robot / cal_sta_robot.c @ 1926

History | View | Annotate | Download (23.4 KB)

1

    
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
#include <dragonfly_lib.h>
33
#include <wl_basic.h>
34
#include <encoders.h>
35
#include "cal_sta_robot.h"
36

    
37
/*** TODO: ***
38

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.
41

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
288
        
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
}
299

    
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);
312
        }
313
        orb_set_color(ORB_OFF);
314
}
315

    
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
}
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
}