Statistics
| Revision:

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

History | View | Annotate | Download (12.8 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(void)
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
void correctTurn(void)
130
{
131
        orb1_set_color(BLUE);                        // BLUE and PURPLE
132
        left(220);
133
        while(1)
134
        {
135
                int bomNum = 0;                                // bomNum is the current maximum reading
136
                bom_refresh(BOM_ALL);
137
                bomNum = bom_get_max();
138
                usb_puti(bomNum);
139
                if(bomNum == 4)                                // when it's turned the right way, stop
140
                {
141
                        timeout = 0;
142
                        //motor_l_set(1, 200);
143
                        //motor_r_set(1, 200);
144
                        break;                                        // exits the while() loop to stop the method
145
                }
146
                else                                                // facing the wrong way
147
                {
148
                        if(bomNum == -1){
149
                                
150
                                timeout++;
151
                                
152
                                if(timeout > 500000)                // if it's been looking too long, move a little bit as it turns
153
                                {
154
                                        motor_r_set(FORWARD, 210);
155
                                        motor_l_set(BACKWARD, 190);
156
                                }
157
                        }
158
                        else if((bomNum >= 12) || (bomNum < 4))
159
                        {
160
                                motor_l_set(FORWARD, 200);
161
                                motor_r_set(BACKWARD, 200);
162
                                timeout = 0;
163
                        }
164
                        else
165
                        {
166
                                motor_l_set(BACKWARD, 200);
167
                                motor_r_set(FORWARD, 200);
168
                                timeout = 0;
169
                        }
170
                }
171
        }
172
        return;
173
}
174

    
175

    
176
/*~~~~~~~~~~~~~~ NEED document
177

178
        Actually, we should just get rid of this.
179
*/
180
void correctApproach(void)
181
{
182
        int bomNum = 0;
183
        bom_refresh(BOM_ALL);
184
        bomNum = bom_get_max();
185
        usb_puti(bomNum);
186
        if(bomNum == 4)
187
        {        
188
                motor_l_set(1, 200);
189
                motor_r_set(1, 200);
190
        }
191
        else
192
        {
193
                if(bomNum == -1){}
194
                else if((bomNum >= 12) || (bomNum < 4))
195
                {
196
                        motor_l_set(FORWARD, 200);
197
                        motor_r_set(BACKWARD, 200);
198
                }
199
                else
200
                {
201
                        motor_l_set(BACKWARD, 200);
202
                        motor_r_set(FORWARD, 200);
203
                }
204
                delay_ms(100);
205
        }
206
}
207

    
208

    
209
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
210
void go_straight(void){                                                // drives forward a hardcoded distance. May not be useful.
211
        forward(200);
212
        encoder_rst_dx(LEFT);
213
        encoder_rst_dx(RIGHT);
214
        delay_ms(100); 
215
        int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
216
        int count = 0;
217
        int d;
218
        while (count<25){                                                //count = 25 when bot6; count <12
219
                x_left = encoder_get_x(LEFT);
220
                x_right = encoder_get_x(RIGHT);
221
                d = x_right-x_left;
222
                if (d>13 || d<-13){
223
                        if(d<50 && d>-50){
224
                                d = round(1.0*d/4);
225
                                setforward(200+d, 200-d);
226
                        }
227
                }
228
                delay_ms(32);
229
                count++;
230
        }
231
}
232

    
233

    
234
/* 
235
    BLINK the given number times
236
*/
237
void blink(int num) {
238
        for(int i = 0; i<num; i++)
239
        {
240
                orb_set_color(ORB_OFF);
241
                delay_ms(200);
242
                orb_set_color(RED);
243
                delay_ms(200);
244
        }
245
        orb_set_color(ORB_OFF);
246
}
247

    
248

    
249

    
250

    
251

    
252
int main(void)
253
{
254
        /* Initialize dragonfly board */
255
            dragonfly_init(ALL_ON);
256
            /* Initialize the basic wireless library */
257
            wl_basic_init_default();
258
            /* Set the XBee channel to your assigned channel */        /* Set the XBee channel to your assigned channel */
259
        wl_set_channel(24);
260

    
261
        int sending_counter = 0;
262
        int robotid = get_robotid();
263
        int used[16];
264
        for (int i=0; i<16; i++) used[i] = 0;
265
        char send_buffer[2];
266
        int data_length;
267
        unsigned char *packet_data=wl_basic_do_default(&data_length);
268
        
269
        
270
        int state = EDGE;
271
        int beacon_State=0;
272
        int waitingCounter=0;
273
        int robotsReceived=0;
274
        
275
        if(wheel()<100)
276
        {
277
                state=EDGE;
278
        }
279
        else
280
        {
281
                state=BEACON;
282
        }
283
        
284
        int distance=1000;                                                // how far away to stop.
285
        int onefoot=300, speed=250;                                // one foot is 490 for bot 1; one foot is 200 for bot6
286
        
287
        while(1)
288
        {
289
                bom_refresh(BOM_ALL);
290
                
291
                /*
292
                *******TERMinology**************
293
                EDGE=0 other names: slave. Definition: robots on the edge of  the circle;
294
                BEACON=1 other name: master. Definition: robots in the center of the circle;
295
                
296
                
297
                *******EXPECTED MOVES ********** 
298
                The designed movement:
299
                 1. one center robot, several edge robots are on;
300
                 2. center robots: button 1 is pressed;
301
                 3. center robots: send global package telling edges that he exists;
302
                 4. EDGE robots response with ACK. 
303
                 5. EDGE robots wait for center robots to finish counting (DONE package)
304
                 6. *******************TODO ***************
305
                */
306
                
307
                
308
                // decide if its is center or not.
309
                switch(state)
310
                {
311
                        /**********
312
                                if  EDGE /slave robots
313
                        */
314
                        case 0:        
315
                                
316
                                /*
317
                                        0. EDGE robots are on. 
318
                                        1. They are waiting for ExiST pacakage from the Center robots
319
                                        2. After they receive the package, they send ACK package to center.
320
                                        3. Wait the center robot to finish counting all EDGE robots
321
                                */
322
                                while(1)   
323
                                {
324
                                        bom_off();
325
                                        orb1_set_color(YELLOW);orb2_set_color(CYAN);
326
                                        packet_data=wl_basic_do_default(&data_length);
327
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
328
                                        {
329
                                                send_buffer[0]=CIRCLE_ACTION_ACK;
330
                                                send_buffer[1]=robotid;
331
                                                
332
                                                wl_basic_send_global_packet(42,send_buffer,2);
333
                                                break;
334
                                        }
335
                                }
336
                                
337
                                /*
338
                                        1. Wait for DONE package 
339
                                        2. The counting process is DONE
340
                                */
341
                                while(1)                
342
                                {
343
                                        orb_set_color(YELLOW);orb2_set_color(PURPLE);
344
                                        packet_data=wl_basic_do_default(&data_length);
345
                                        wl_basic_send_global_packet(42,send_buffer,2);
346
                                        
347
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
348
                                        {
349
                                                break;
350
                                        }
351
                                }
352
                                
353
                                
354
                                // COLOR afer DONE ---> MAGENTA
355
                                orb_set_color(MAGENTA);
356
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
357
                                correctTurn();                        // turn to face the beacon
358
                                forward(220);
359
                                //range_init();
360
                                
361
                                
362
                                distance = get_distance();
363
                                while (distance>=onefoot || distance==0)
364
                                {
365
                                        if(distance==0)
366
                                                orb_set_color(WHITE);
367
                                        //correctApproach();
368
                                        distance = get_distance();
369
                                        delay_ms(14);
370
                                }
371

    
372
                                stop();
373
                                orb_set_color(LIME);
374
                                
375
                                send_buffer[0]=CIRCLE_ACTION_ACK;
376
                                send_buffer[1]=robotid;        
377
                                wl_basic_send_global_packet(42,send_buffer,2);
378
                                
379
                                // NEW CODE FROM JOEL
380
                                
381
                                while(1)
382
                                {
383
                                        packet_data = wl_basic_do_default(&data_length);
384
                                        
385
                                        if(packet_data[0]=='s') stop2=1;
386
                                        if(packet_data[0]=='a') switch_sending();
387
                                                
388
                                        if(sending)
389
                                        {
390
                                        
391
                                        }
392
                                
393
                                        else // recieving
394
                                        {
395
                                
396
                                                if(stop2)
397
                                                {
398
                                                        motor_l_set(FORWARD,0);
399
                                                        motor_r_set(FORWARD,0);
400
                                                        orb1_set_color(GREEN);
401
                                                }
402
                                
403
                                                else
404
                                                {
405
                                                        int max_bom = bom_get_max();
406
                                                                /*usb_puts("bom_get_max : ");
407
                                                        usb_puti(max_bom);
408
                                                        usb_puts("/n/r");*/
409
                                                
410
                                                
411
                                                        if(max_bom == 8)
412
                                                        {        
413
                                                                orb2_set_color(BLUE);
414
                                                                motor_r_set(FORWARD,180);
415
                                                                motor_l_set(FORWARD,180);
416
                                                                
417
                                                        }
418
                                                        else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
419
                                                        {
420
                                                                motor_l_set(FORWARD,180);
421
                                                                motor_r_set(FORWARD,0);
422
                                                        }
423
                                                        else if(max_bom == -1);
424
                                                        else 
425
                                                        {
426
                                                                orb2_set_color(GREEN);
427
                                                                motor_l_set(FORWARD,0);
428
                                                                motor_r_set(FORWARD,180);
429
                                                        }
430
                                                }
431
                                        
432
                                        }
433
                                
434
                                delay_ms(10);
435
                                
436
                                } //end while
437
                                break;
438
                                
439
                                // END for EDGE robots
440
                        
441

    
442

    
443

    
444

    
445
        
446
                        /***************
447
                           if  The CENTER/BEACON/MASTER robot
448
                        */        
449
                        case 1:                        // BEACON /master /enter robots
450
                                switch(beacon_State) {
451
                                /*
452
                                   1. center  robots on wait for pressing button 1
453
                                */
454
                                case 0:                
455
                                        bom_on();
456
                                        orb_set_color(PURPLE);
457
                                        if(button1_click()) beacon_State=1;
458
                                        break;        
459
                                /*
460
                                        1. Send EXIST package to EDGE robots
461
                                */
462
                                case 1:                // sends a global exist packet to see how many robots there are
463
                                        orb_set_color(RED);
464
                                        send_buffer[0]=CIRCLE_ACTION_EXIST;
465
                                        send_buffer[1]=get_robotid();
466
                                        wl_basic_send_global_packet(42,send_buffer,2);
467
                                        beacon_State=2;
468
                                        break;
469
                                /*
470
                                        1. Count the number of the EDGE robots 
471
                                        *******NOTE: at most  1500  times of loop ******
472
                                */
473
                                case 2:         
474
                                        waitingCounter++;
475
                                        orb1_set_color(YELLOW);
476
                                        orb2_set_color(BLUE);
477
                                        packet_data=wl_basic_do_default(&data_length);
478
                                        if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
479
                                        {
480
                                                orb_set_color(RED);
481
                                                orb2_set_color(BLUE);
482
                                                //only add to robots seen if you haven't gotten an ACK from this robot
483
                                                if(used[packet_data[1]]==0){                
484
                                                        robotsReceived++;
485
                                                        used[packet_data[1]]=1;
486
                                                }
487
                                        }
488
                                        if(waitingCounter >= 1500){
489
                                                beacon_State=3;
490
                                        }
491
                                        break;
492
                                /*
493
                                        COUNTing is DONE.
494
                                        Sending DONE package.
495
                                */        
496
                                case 3:
497
                                        blink(robotsReceived);
498
                                        orb_set_color(GREEN);
499
                                        send_buffer[0]=CIRCLE_ACTION_DONE;
500
                                        wl_basic_send_global_packet(42,send_buffer,2);
501
                                        beacon_State=4;
502
                                        break;
503
                                /*
504
                                        Wait for all the robots to get to right distance/position 
505
                                */
506
                                case 4: 
507
                                        orb1_set_color(YELLOW);
508
                                        orb2_set_color(WHITE);
509
                                        
510
                                        int numOk = 0;
511
                                        
512
                                        while(numOk<robotsReceived)
513
                                        {
514
                                                packet_data=wl_basic_do_default(&data_length);
515
                                                if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
516
                                                {
517
                                                        numOk++;
518
                                                }
519
                                        }
520
                                        
521
                                        beacon_State = 5;
522
                                        break; // <----------------------------------------------------------Echo wrote this "break". need to be checked. 
523
                                
524
                                /**
525
                                    NEED to be documented
526
                                */
527
                                case 5:
528
                                        orb_set_color(BLUE);
529
                                        // clock for switching the BOMs between the master and slave
530
                                        if(sending_counter++>4)        
531
                                        {
532
                                                switch_sending();
533
                                                sending_counter = 0;
534
                                                send_buffer[0] = 'a';
535
                                                wl_basic_send_global_packet(42, send_buffer, 1);
536
                                        }
537
                                        
538
                                        
539
                                        if(sending)
540
                                        {
541
                                                
542
                                        }
543
                                        
544
                                        else // recieving
545
                                        {
546
                                                int max_bom = bom_get_max();
547
                                                usb_puts("bom_get_max : ");
548
                                                usb_puti(max_bom);
549
                                                usb_puts("\n\r");
550
                                                
551
                                                if(max_bom == desired_max_bom)
552
                                                {
553
                                                        // only stops the slave if two consecutive boms 
554
                                                        //     reading give the desired bom as the max one. Filters the noise.
555
                                                        if(bom_max_counter)                 
556
                                                        {
557
                                                                send_buffer[0] = 's';
558
                                                                wl_basic_send_global_packet(42, send_buffer, 2);
559
                                                        }
560
                                                        bom_max_counter =1;
561
                                                }
562
                                                else bom_max_counter = 0;
563
                                                
564
                                        }
565
                                        
566
                                        break;
567
                        }
568
                }
569
        }
570

    
571
        orb_set_color(RED);
572
        while(1); /* END HERE */
573

    
574
        //return 0;
575
}