Statistics
| Revision:

root / trunk / code / behaviors / formation_control / circle / circle.c @ 1626

History | View | Annotate | Download (13.6 KB)

1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include <encoders.h>
4
#include "circle.h"
5

    
6
int EDGE = 0;
7
int BEACON = 1;
8
int timeout = 0;
9
int sending = 0;
10
int stop2 = 0;
11
struct vector slave_position;
12
int desired_max_bom;
13
int bom_max_counter;
14

    
15

    
16
void set_desired_max_bom(int desired_angle)
17
{
18
        if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;
19
        if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;
20
        if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;
21
        if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;
22
        if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;
23
        if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;
24
        if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;
25
        if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;
26
        if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;
27
        if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;
28
        if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;
29
        if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;
30
        if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;
31
        if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;
32
        if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;
33
        if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;
34
}
35

    
36
void switch_sending ()
37
{
38
        if(sending)
39
        {
40
                sending = 0;
41
                bom_off();
42
        }
43
        else
44
        {
45
                sending = 1;
46
                bom_on();
47
        }
48
}
49

    
50

    
51
/*
52
        This program is used to make robots target a center (leader) robot using the BOM,
53
        then drive toward it and stop at a certain distance away.
54
        
55
        The distance will eventually be adjustable.
56

57
        With adjustment, the leader robot will be able to turn and use its standardized
58
        rangefinder to reposition or space the robots evenly.
59
        
60
        AuTHORS: Nico, Alex, Reva, Echo, Steve
61
*/
62

    
63

    
64
/* 
65
TODO:
66
        Used: Bots 1, 7
67
        16 Performed Badly
68
        12 worked ok as beacon, not well as edge
69
        
70
                Fix orient code so the bot does not toggle back and forth when it tries to turn
71
                
72
                Use the center bot to check distances
73
Done-->        Count them ("spam" method)
74
                Use beacon to find relative positions
75
                Beacon tells them how to move to be at the right distance
76
                *done*Wireless communication, initialization
77
*/
78

    
79

    
80

    
81
void forward(int speed){                        // set the motors to this forward speed.
82
        motor_l_set(FORWARD,speed);
83
        motor_r_set(FORWARD,speed);
84
}
85
void left(int speed){                                // turn left at this speed.
86
        motor_l_set(FORWARD,speed);
87
        motor_r_set(BACKWARD,speed);
88
}
89
void right(int speed){
90
        motor_l_set(BACKWARD,speed);
91
        motor_r_set(FORWARD,speed);
92
}
93
void stop(void){                                        // could be set to motors_off(), or just use this as an alternative.
94
        motor_l_set(BACKWARD,0);
95
        motor_r_set(FORWARD,0);
96
}
97
void setforward(int spd1, int spd2){
98
        motor_l_set(FORWARD,spd1);
99
        motor_r_set(FORWARD,spd2);
100
}
101
void backward(int speed){
102
        motor_l_set(BACKWARD, speed);
103
        motor_r_set(BACKWARD, speed);
104
}
105
int get_distance(void){                                // takes an averaged reading of the front rangefinder
106
        int temp,distance,kk=5;                        // kk sets this to 5 readings.
107
        distance =0;
108
        for (int i=0; i<kk; i++){
109
                temp = range_read_distance(IR2);
110
                if (temp == -1)
111
                {
112
                        //temp=0;
113
                        i--;
114
                }
115
                else
116
                        distance+= temp;
117
                delay_ms(3);
118
        }
119
        if (kk>0)
120
                return (int)(distance/kk);
121
        else 
122
                return 0;
123
}
124

    
125

    
126

    
127
/*~~~~~~~~~~~~~ NEED document
128
*/
129
int correctTurn(void)
130
{
131
        int bomNum = 0;
132
        bom_refresh(BOM_ALL);
133
        bomNum = bom_get_max();
134
        usb_puti(bomNum);
135
        if(bomNum == 4)
136
        {        timeout = 0;
137
                //motor_l_set(1, 200);
138
                //motor_r_set(1, 200);
139
                return 1; //<--------------------------------------------1 and 0 are switched
140
        }
141
        else
142
        {
143
                if(bomNum == -1){
144
                        timeout++;
145
                        if(timeout > 500000)
146
                        {
147
                                motor_r_set(FORWARD, 210);
148
                                motor_l_set(BACKWARD, 190);
149
                        }
150
                }
151
                else if((bomNum >= 12) || (bomNum < 4))
152
                {
153
                        motor_l_set(FORWARD, 200);timeout = 0;
154
                        motor_r_set(BACKWARD, 200);
155
                }
156
                else
157
                {
158
                        motor_l_set(BACKWARD, 200);timeout = 0;
159
                        motor_r_set(FORWARD, 200);
160
                }
161
        }
162
        return 0;
163
}
164

    
165

    
166
/*~~~~~~~~~~~~~~ NEED document
167
*/
168
void correctApproach(void)
169
{
170
        int bomNum = 0;
171
        bom_refresh(BOM_ALL);
172
        bomNum = bom_get_max();
173
        usb_puti(bomNum);
174
        if(bomNum == 4)
175
        {        
176
                motor_l_set(1, 200);
177
                motor_r_set(1, 200);
178
        }
179
        else
180
        {
181
                if(bomNum == -1){}
182
                else if((bomNum >= 12) || (bomNum < 4))
183
                {
184
                        motor_l_set(FORWARD, 200);
185
                        motor_r_set(BACKWARD, 200);
186
                }
187
                else
188
                {
189
                        motor_l_set(BACKWARD, 200);
190
                        motor_r_set(FORWARD, 200);
191
                }
192
                delay_ms(100);
193
        }
194
}
195

    
196

    
197
/*
198
  Start turning  until the front receive the MAx bom reading
199
*/
200
void turn_to_beacon(int max){
201
        if (max>-1 && max<16){
202
                int index = (max+12)%16;
203
                if (index==0) { 
204
                        stop(); 
205
                }
206
                else if (index<8) right(170);
207
                else left(170);
208
        }
209
}
210

    
211

    
212
/*
213
        Start turning its fron to the MAx bom reading
214
        But it won't stop by itself.
215
*/
216
void turn_to_beacon2(int max){                                // like the previous but no stop() call'
217

    
218

    
219

    
220

    
221
        if (max>-1 && max<16){
222
                int index = (max+12)%16;
223
                if (index==0) { 
224
                         
225
                }
226
                else if (index<8) right(170);
227
                else left(170);
228
        }
229
}
230

    
231

    
232

    
233

    
234
/*
235
        Turn towards the MAX bom direction  
236
        until the front receive the MAx bom reading
237
*/
238
void orient(void){
239
        int max_index = -1;
240
        while (max_index!=4) {
241
                /* Refresh and make sure the table is updated */
242
                bom_refresh(BOM_ALL);
243
                max_index = bom_get_max();
244
                turn_to_beacon(max_index);
245
                delay_ms(22);
246
        }
247
}
248

    
249

    
250
/*
251
        Turn towards the MAX bom direction  
252
        until the front receive the MAx bom reading
253
        
254
        This function has less stop() so the robots run more smooth
255
*/
256
void orient2(void){
257
        int max_index = -1;
258
        while (max_index!=4) {
259
                /* Refresh and make sure the table is updated */
260
                bom_refresh(BOM_ALL);
261
                max_index = bom_get_max();
262
                turn_to_beacon2(max_index);
263
                delay_ms(22);
264
        }
265
}
266
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
267
void go_straight(void){                                                // drives forward a hardcoded distance. May not be useful.
268
        forward(200);
269
        encoder_rst_dx(LEFT);
270
        encoder_rst_dx(RIGHT);
271
        delay_ms(100); 
272
        int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
273
        int count = 0;
274
        int d;
275
        while (count<25){                                                //count = 25 when bot6; count <12
276
                x_left = encoder_get_x(LEFT);
277
                x_right = encoder_get_x(RIGHT);
278
                d = x_right-x_left;
279
                if (d>13 || d<-13){
280
                        if(d<50 && d>-50){
281
                                d = round(1.0*d/4);
282
                                setforward(200+d, 200-d);
283
                        }
284
                }
285
                delay_ms(32);
286
                count++;
287
        }
288
}
289

    
290

    
291
/* 
292
    BLINK the given number times
293
*/
294
void blink(int num) {
295
        for(int i = 0; i<num; i++)
296
        {
297
                orb_set_color(ORB_OFF);
298
                delay_ms(200);
299
                orb_set_color(RED);
300
                delay_ms(200);
301
        }
302
        orb_set_color(ORB_OFF);
303
}
304

    
305

    
306

    
307

    
308

    
309
int main(void)
310
{
311
        /* Initialize dragonfly board */
312
            dragonfly_init(ALL_ON);
313
            /* Initialize the basic wireless library */
314
            wl_basic_init_default();
315
            /* Set the XBee channel to your assigned channel */        /* Set the XBee channel to your assigned channel */
316
        wl_set_channel(24);
317

    
318
        int sending_counter = 0;
319
        int robotid = get_robotid();
320
        int used[16];
321
        for (int i=0; i<16; i++) used[i] = 0;
322
        char send_buffer[2];
323
        int data_length;
324
        unsigned char *packet_data=wl_basic_do_default(&data_length);
325
        
326
        
327
        int state = EDGE;
328
        int beacon_State=0;
329
        int waitingCounter=0;
330
        int robotsReceived=0;
331
        if(wheel()<100)
332
        {
333
                state=EDGE;
334
        }
335
        else
336
        {
337
                state=BEACON;
338
        }
339
        
340
        int distance=1000;                                                // how far away to stop.
341
        int onefoot=300, speed=250;                                // one foot is 490 for bot 1; one foot is 200 for bot6
342
        
343
        while(1)
344
        {
345
                bom_refresh(BOM_ALL);
346
                
347
                /*
348
                *******TERMinology**************
349
                EDGE=0 other names: slave. Definition: robots on the edge of  the circle;
350
                BEACON=1 other name: master. Definition: robots in the center of the circle;
351
                
352
                
353
                *******EXPECTED MOVES ********** 
354
                The designed movement:
355
                 1. one center robot, several edge robots are on;
356
                 2. center robots: button 1 is pressed;
357
                 3. center robots: send global package telling edges that he exists;
358
                 4. EDGE robots response with ACK. 
359
                 5. EDGE robots wait for center robots to finish counting (DONE package)
360
                 6. *******************TODO ***************
361
                */
362
                
363
                
364
                // decide if its is center or not.
365
                switch(state)
366
                {
367
                        /**********
368
                                if  EDGE /slave robots
369
                        */
370
                        case 0:        
371
                                
372
                                /*
373
                                        0. EDGE robots are on. 
374
                                        1. They are waiting for ExiST pacakage from the Center robots
375
                                        2. After they receive the package, they send ACK package to center.
376
                                        3. Wait the center robot to finish counting all EDGE robots
377
                                */
378
                                while(1)   
379
                                {
380
                                        bom_off();
381
                                        orb1_set_color(YELLOW);orb2_set_color(CYAN);
382
                                        packet_data=wl_basic_do_default(&data_length);
383
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
384
                                        {
385
                                                send_buffer[0]=CIRCLE_ACTION_ACK;
386
                                                send_buffer[1]=robotid;
387
                                                
388
                                                wl_basic_send_global_packet(42,send_buffer,2);
389
                                                break;
390
                                        }
391
                                }
392
                                
393
                                /*
394
                                        1. Wait for DONE package 
395
                                        2. The counting process is DONE
396
                                */
397
                                while(1)                
398
                                {
399
                                        orb_set_color(YELLOW);orb2_set_color(PURPLE);
400
                                        packet_data=wl_basic_do_default(&data_length);
401
                                        wl_basic_send_global_packet(42,send_buffer,2);
402
                                        
403
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
404
                                        {
405
                                                break;
406
                                        }
407
                                }
408
                                
409
                                
410
                                // COLOR afer DONE ---> MAGENTA
411
                                orb_set_color(MAGENTA);
412
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
413
                                while(correctTurn()){                                
414
                                }
415
                                forward(220);
416
                                //range_init();
417
                                
418
                                
419
                                distance = get_distance();
420
                                while (distance>=onefoot || distance==0)
421
                                {
422
                                        if(distance==0)
423
                                                orb_set_color(WHITE);
424
                                        //correctApproach();
425
                                        distance = get_distance();
426
                                        delay_ms(14);
427
                                }
428

    
429
                                stop();
430
                                orb_set_color(LIME);
431
                                
432
                                send_buffer[0]=CIRCLE_ACTION_ACK;
433
                                send_buffer[1]=robotid;        
434
                                wl_basic_send_global_packet(42,send_buffer,2);
435
                                
436
                                // NEW CODE FROM JOEL
437
                                
438
                                while(1)
439
                                {
440
                                        packet_data = wl_basic_do_default(&data_length);
441
                                        
442
                                        if(packet_data[0]=='s') stop2=1;
443
                                        if(packet_data[0]=='a') switch_sending();
444
                                                
445
                                        if(sending)
446
                                        {
447
                                        
448
                                        }
449
                                
450
                                        else // recieving
451
                                        {
452
                                
453
                                                if(stop2)
454
                                                {
455
                                                        motor_l_set(FORWARD,0);
456
                                                        motor_r_set(FORWARD,0);
457
                                                        orb1_set_color(GREEN);
458
                                                }
459
                                
460
                                                else
461
                                                {
462
                                                        int max_bom = bom_get_max();
463
                                                                /*usb_puts("bom_get_max : ");
464
                                                        usb_puti(max_bom);
465
                                                        usb_puts("/n/r");*/
466
                                                
467
                                                
468
                                                        if(max_bom == 8)
469
                                                        {        
470
                                                                orb2_set_color(BLUE);
471
                                                                motor_r_set(FORWARD,180);
472
                                                                motor_l_set(FORWARD,180);
473
                                                                
474
                                                        }
475
                                                        else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
476
                                                        {
477
                                                                motor_l_set(FORWARD,180);
478
                                                                motor_r_set(FORWARD,0);
479
                                                        }
480
                                                        else if(max_bom == -1);
481
                                                        else 
482
                                                        {
483
                                                                orb2_set_color(GREEN);
484
                                                                motor_l_set(FORWARD,0);
485
                                                                motor_r_set(FORWARD,180);
486
                                                        }
487
                                                }
488
                                        
489
                                        }
490
                                
491
                                delay_ms(10);
492
                                
493
                                } //end while
494
                                break;
495
                                
496
                                // END for EDGE robots
497
                        
498

    
499

    
500

    
501

    
502
        
503
                        /***************
504
                           if  The CENTER/BEACON/MASTER robot
505
                        */        
506
                        case 1:                        // BEACON /master /enter robots
507
                                switch(beacon_State) {
508
                                /*
509
                                   1. center  robots on wait for pressing button 1
510
                                */
511
                                case 0:                
512
                                        bom_on();
513
                                        orb_set_color(PURPLE);
514
                                        if(button1_click()) beacon_State=1;
515
                                        break;        
516
                                /*
517
                                        1. Send EXIST package to EDGE robots
518
                                */
519
                                case 1:                // sends a global exist packet to see how many robots there are
520
                                        orb_set_color(RED);
521
                                        send_buffer[0]=CIRCLE_ACTION_EXIST;
522
                                        send_buffer[1]=get_robotid();
523
                                        wl_basic_send_global_packet(42,send_buffer,2);
524
                                        beacon_State=2;
525
                                        break;
526
                                /*
527
                                        1. Count the number of the EDGE robots 
528
                                        *******NOTE: at most  1500  times of loop ******
529
                                */
530
                                case 2:         
531
                                        waitingCounter++;
532
                                        orb1_set_color(YELLOW);
533
                                        orb2_set_color(BLUE);
534
                                        packet_data=wl_basic_do_default(&data_length);
535
                                        if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
536
                                        {
537
                                                orb_set_color(RED);orb2_set_color(BLUE);
538
                                                //only add to robots seen if you haven't gotten an ACK from this robot
539
                                                if(used[packet_data[1]]==0){                
540
                                                        robotsReceived++;
541
                                                        used[packet_data[1]]=1;
542
                                                }
543
                                        }
544
                                        if(waitingCounter >= 1500){
545
                                                beacon_State=3;
546
                                        }
547
                                        break;
548
                                /*
549
                                        COUNTing is DONE.
550
                                        Sending DONE package.
551
                                */        
552
                                case 3:
553
                                        orb_set_color(GREEN);
554
                                        send_buffer[0]=CIRCLE_ACTION_DONE;
555
                                        wl_basic_send_global_packet(42,send_buffer,2);
556
                                        beacon_State=4;
557
                                        break;
558
                                /*
559
                                        Wait for all the robots to get to right distance/position 
560
                                */
561
                                case 4: 
562
                                        orb1_set_color(YELLOW);
563
                                        orb2_set_color(WHITE);
564
                                        
565
                                        int numOk = 0;
566
                                        
567
                                        while(numOk<robotsReceived)
568
                                        {
569
                                                packet_data=wl_basic_do_default(&data_length);
570
                                                if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
571
                                                {
572
                                                        numOk++;
573
                                                }
574
                                        }
575
                                        
576
                                        beacon_State = 5;
577
                                        break; // <----------------------------------------------------------Echo wrote this "break". need to be checked. 
578
                                
579
                                /**
580
                                    NEED to be documented
581
                                */
582
                                case 5:
583
                                        orb_set_color(BLUE);
584
                                        // clock for switching the BOMs between the master and slave
585
                                        if(sending_counter++>4)        
586
                                        {
587
                                                switch_sending();
588
                                                sending_counter = 0;
589
                                                send_buffer[0] = 'a';
590
                                                wl_basic_send_global_packet(42, send_buffer, 1);
591
                                        }
592
                                        
593
                                        
594
                                        if(sending)
595
                                        {
596
                                                
597
                                        }
598
                                        
599
                                        else // recieving
600
                                        {
601
                                                int max_bom = bom_get_max();
602
                                                usb_puts("bom_get_max : ");
603
                                                usb_puti(max_bom);
604
                                                usb_puts("\n\r");
605
                                                
606
                                                if(max_bom == desired_max_bom)
607
                                                {
608
                                                        // only stops the slave if two consecutive boms 
609
                                                        //     reading give the desired bom as the max one. Filters the noise.
610
                                                        if(bom_max_counter)                 
611
                                                        {
612
                                                                send_buffer[0] = 's';
613
                                                                wl_basic_send_global_packet(42, send_buffer, 2);
614
                                                        }
615
                                                        bom_max_counter =1;
616
                                                }
617
                                                else bom_max_counter = 0;
618
                                                
619
                                        }
620
                                        
621
                                        break;
622
                        }
623
                }
624
        }
625

    
626
        orb_set_color(RED);
627
        while(1); /* END HERE */
628

    
629
        //return 0;
630
}