Statistics
| Revision:

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

History | View | Annotate | Download (13.3 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
//Last used 12,13,7(BOM)
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 edge_State=0;
273
        int waitingCounter=0;
274
        int robotsReceived=0;
275
        int offset = 20, time=0;
276
        
277
        if(wheel()<100)
278
        {
279
                state=EDGE;
280
        }
281
        else
282
        {
283
                state=BEACON;
284
        }
285
        
286
        int distance=1000;                                                // how far away to stop.
287
        int onefoot=250, speed=250;                                // one foot is 490 for bot 1; one foot is 200 for bot6
288
        
289
        while(1)
290
        {
291
                bom_refresh(BOM_ALL);
292
                
293
                /*
294
                *******TERMinology**************
295
                EDGE=0 other names: slave. Definition: robots on the edge of  the circle;
296
                BEACON=1 other name: master. Definition: robots in the center of the circle;
297
                
298
                
299
                *******EXPECTED MOVES ********** 
300
                The designed movement:
301
                 1. one center robot, several edge robots are on;
302
                 2. center robots: button 1 is pressed;
303
                 3. center robots: send global package telling edges that he exists;
304
                 4. EDGE robots response with ACK. 
305
                 5. EDGE robots wait for center robots to finish counting (DONE package)
306
                 6. *******************TODO ***************
307
                */
308
                
309
                
310
                // decide if its is center or not.
311
                switch(state)
312
                {
313
                        /**********
314
                                if  EDGE /slave robots
315
                        */
316
                        case 0:        
317
                                switch(edge_State)
318
                                {
319
                                        /*
320
                                                0. EDGE robots are on. 
321
                                                1. They are waiting for ExiST pacakage from the Center robots
322
                                                2. After they receive the package, they send ACK package to center.
323
                                                3. Wait the center robot to finish counting all EDGE robots
324
                                        */
325
                                        case 0:   
326
                                                bom_off();
327
                                                orb1_set_color(YELLOW);orb2_set_color(CYAN);
328
                                                packet_data=wl_basic_do_default(&data_length);
329
                                                if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
330
                                                {
331
                                                        send_buffer[0]=CIRCLE_ACTION_ACK;
332
                                                        send_buffer[1]=robotid;
333
                                                        
334
                                                        wl_basic_send_global_packet(42,send_buffer,2);
335
                                                        edge_State=1;
336
                                                }
337
                                        break;
338
                                        /*
339
                                                1. Wait for DONE package 
340
                                                2. The counting process is DONE
341
                                        */
342
                                        case 1:                
343
                                        
344
                                                orb_set_color(YELLOW);orb2_set_color(PURPLE);
345
                                                packet_data=wl_basic_do_default(&data_length);
346
                                                //wl_basic_send_global_packet(42,send_buffer,2);
347
                                                
348
                                                if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
349
                                                {
350
                                                        edge_State=2;
351
                                                }
352
                                        break;
353
                                        
354
                                        case 2:
355
                                                // COLOR afer DONE ---> MAGENTA
356
                                                orb_set_color(MAGENTA);
357
                                                correctTurn();                        // turn to face the beacon
358
                                                forward(175);
359
                                                //range_init();
360
                                                
361
                                                
362
                                                distance = get_distance();
363
                                                time=0;
364
                                                while ((distance-offset)>=onefoot || distance==0 || (distance+offset)<onefoot)
365
                                                {
366
                                                        if(distance==0)
367
                                                                orb_set_color(WHITE);
368
                                                        else if(distance-offset>=onefoot)
369
                                                                forward(175);
370
                                                        else
371
                                                                backward(175);
372
                                                        //correctApproach();
373
                                                        distance = get_distance();
374
                                                        delay_ms(14);
375
                                                        time+=14;
376
                                                        if(time>500){
377
                                                                correctTurn();
378
                                                                time=0;
379
                                                        }
380
                                                }
381
                                                        
382
                                                stop();
383
                                                orb_set_color(LIME);
384
                                                edge_State=3;
385
                                        break;
386
                                        
387
                                        case 3:
388
                                        //do nothing
389
                                        break;
390
                                }
391
                                /*send_buffer[0]=CIRCLE_ACTION_ACK;
392
                                send_buffer[1]=robotid;        
393
                                wl_basic_send_global_packet(42,send_buffer,2);
394
                                
395
                                // NEW CODE FROM JOEL
396
                                
397
                                while(1)
398
                                {
399
                                        packet_data = wl_basic_do_default(&data_length);
400
                                        
401
                                        if(packet_data[0]=='s') stop2=1;
402
                                        if(packet_data[0]=='a') switch_sending();
403
                                                
404
                                        if(sending)
405
                                        {
406
                                        
407
                                        }
408
                                
409
                                        else // recieving
410
                                        {
411
                                
412
                                                if(stop2)
413
                                                {
414
                                                        motor_l_set(FORWARD,0);
415
                                                        motor_r_set(FORWARD,0);
416
                                                        orb1_set_color(GREEN);
417
                                                }
418
                                
419
                                                else
420
                                                {
421
                                                        int max_bom = bom_get_max();
422
                                                                /*usb_puts("bom_get_max : ");
423
                                                        usb_puti(max_bom);
424
                                                        usb_puts("/n/r");*/
425
                                        /*        
426
                                                
427
                                                        if(max_bom == 8)
428
                                                        {        
429
                                                                orb2_set_color(BLUE);
430
                                                                motor_r_set(FORWARD,180);
431
                                                                motor_l_set(FORWARD,180);
432
                                                                
433
                                                        }
434
                                                        else if(max_bom == 7 || max_bom == 6 || max_bom == 5)
435
                                                        {
436
                                                                motor_l_set(FORWARD,180);
437
                                                                motor_r_set(FORWARD,0);
438
                                                        }
439
                                                        else if(max_bom == -1);
440
                                                        else 
441
                                                        {
442
                                                                orb2_set_color(GREEN);
443
                                                                motor_l_set(FORWARD,0);
444
                                                                motor_r_set(FORWARD,180);
445
                                                        }
446
                                                }
447
                                        
448
                                        }
449
                                
450
                                delay_ms(10);
451
                                
452
                                } //end while
453
                                */break;
454
                                
455
                                // END for EDGE robots
456
                        
457

    
458

    
459

    
460

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

    
587
        orb_set_color(RED);
588
        while(1); /* END HERE */
589

    
590
        //return 0;
591
}