Statistics
| Revision:

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

History | View | Annotate | Download (7.97 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

    
10

    
11
/*
12
        This program is used to make robots target a center (leader) robot using the BOM,
13
        then drive toward it and stop at a certain distance away.
14
        
15
        The distance will eventually be adjustable.
16

17
        With adjustment, the leader robot will be able to turn and use its standardized
18
        rangefinder to reposition or space the robots evenly.
19
        
20
        AuTHORS: Nico, Alex, Reva, Echo, Steve
21
*/
22

    
23

    
24
/* 
25
TODO:
26
        Used: Bots 1, 7
27
        16 Performed Badly
28
        12 worked ok as beacon, not well as edge
29
        
30
                Fix orient code so the bot does not toggle back and forth when it tries to turn
31
                
32
                Use the center bot to check distances
33
Done-->        Count them ("spam" method)
34
                Use beacon to find relative positions
35
                Beacon tells them how to move to be at the right distance
36
                *done*Wireless communication, initialization
37
*/
38

    
39

    
40

    
41
void forward(int speed){                        // set the motors to this forward speed.
42
        motor_l_set(FORWARD,speed);
43
        motor_r_set(FORWARD,speed);
44
}
45
void left(int speed){                                // turn left at this speed.
46
        motor_l_set(FORWARD,speed);
47
        motor_r_set(BACKWARD,speed);
48
}
49
void right(int speed){
50
        motor_l_set(BACKWARD,speed);
51
        motor_r_set(FORWARD,speed);
52
}
53
void stop(void){                                        // could be set to motors_off(), or just use this as an alternative.
54
        motor_l_set(BACKWARD,0);
55
        motor_r_set(FORWARD,0);
56
}
57
void setforward(int spd1, int spd2){
58
        motor_l_set(FORWARD,spd1);
59
        motor_r_set(FORWARD,spd2);
60
}
61
void backward(int speed){
62
        motor_l_set(BACKWARD, speed);
63
        motor_r_set(BACKWARD, speed);
64
}
65
int get_distance(void){                                // takes an averaged reading of the front rangefinder
66
        int temp,distance,kk=5;                        // kk sets this to 5 readings.
67
        distance =0;
68
        for (int i=0; i<kk; i++){
69
                temp = range_read_distance(IR2);
70
                if (temp == -1)
71
                {
72
                        //temp=0;
73
                        i--;
74
                }
75
                else
76
                        distance+= temp;
77
                delay_ms(3);
78
        }
79
        if (kk>0)
80
                return (int)(distance/kk);
81
        else 
82
                return 0;
83
}
84

    
85

    
86
int correctTurn(void)
87
{
88
        int bomNum = 0;
89
        bom_refresh(BOM_ALL);
90
        bomNum = bom_get_max();
91
        usb_puti(bomNum);
92
        if(bomNum == 4)
93
        {        timeout = 0;
94
                motor_l_set(1, 200);
95
                motor_r_set(1, 200);
96
                return 0;
97
        }
98
        else
99
        {
100
                if(bomNum == -1){
101
                        timeout++;
102
                        if(timeout > 500000)
103
                        {
104
                                motor_r_set(FORWARD, 210);
105
                                motor_l_set(BACKWARD, 190);
106
                        }
107
                }
108
                else if((bomNum >= 12) || (bomNum < 4))
109
                {
110
                        motor_l_set(FORWARD, 200);timeout = 0;
111
                        motor_r_set(BACKWARD, 200);
112
                }
113
                else
114
                {
115
                        motor_l_set(BACKWARD, 200);timeout = 0;
116
                        motor_r_set(FORWARD, 200);
117
                }
118
        }
119
        return 1;
120
}
121

    
122
void correctApproach(void)
123
{
124
        int bomNum = 0;
125
        bom_refresh(BOM_ALL);
126
        bomNum = bom_get_max();
127
        usb_puti(bomNum);
128
        if(bomNum == 4)
129
        {        
130
                motor_l_set(1, 200);
131
                motor_r_set(1, 200);
132
        }
133
        else
134
        {
135
                if(bomNum == -1){}
136
                else if((bomNum >= 12) || (bomNum < 4))
137
                {
138
                        motor_l_set(FORWARD, 200);
139
                        motor_r_set(BACKWARD, 200);
140
                }
141
                else
142
                {
143
                        motor_l_set(BACKWARD, 200);
144
                        motor_r_set(FORWARD, 200);
145
                }
146
                delay_ms(100);
147
        }
148
}
149

    
150
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
151
void turn_to_beacon(int max){
152
        if (max>-1 && max<16){
153
                int index = (max+12)%16;
154
                if (index==0) { 
155
                        stop(); 
156
                }
157
                else if (index<8) right(170);
158
                else left(170);
159
        }
160
}
161

    
162
void turn_to_beacon2(int max){                                // like the previous but no stop() call'
163

    
164

    
165

    
166

    
167
        if (max>-1 && max<16){
168
                int index = (max+12)%16;
169
                if (index==0) { 
170
                         
171
                }
172
                else if (index<8) right(170);
173
                else left(170);
174
        }
175
}
176

    
177
void orient(void){
178
        int max_index = -1;
179
        while (max_index!=4) {
180
                /* Refresh and make sure the table is updated */
181
                bom_refresh(BOM_ALL);
182
                max_index = bom_get_max();
183
                turn_to_beacon(max_index);
184
                delay_ms(22);
185
        }
186
}
187
void orient2(void){
188
        int max_index = -1;
189
        while (max_index!=4) {
190
                /* Refresh and make sure the table is updated */
191
                bom_refresh(BOM_ALL);
192
                max_index = bom_get_max();
193
                turn_to_beacon2(max_index);
194
                delay_ms(22);
195
        }
196
}
197
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
198
void go_straight(void){                                                // drives forward a hardcoded distance. May not be useful.
199
        forward(200);
200
        encoder_rst_dx(LEFT);
201
        encoder_rst_dx(RIGHT);
202
        delay_ms(100); 
203
        int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
204
        int count = 0;
205
        int d;
206
        while (count<25){                                                //count = 25 when bot6; count <12
207
                x_left = encoder_get_x(LEFT);
208
                x_right = encoder_get_x(RIGHT);
209
                d = x_right-x_left;
210
                if (d>13 || d<-13){
211
                        if(d<50 && d>-50){
212
                                d = round(1.0*d/4);
213
                                setforward(200+d, 200-d);
214
                        }
215
                }
216
                delay_ms(32);
217
                count++;
218
        }
219
}
220
void go_straight_onefoot(void){                                // essentially, another name for the above.  Should be removed.
221
        go_straight();
222
}
223

    
224
void blink(int num) {
225
        for(int i = 0; i<num; i++)
226
        {
227
                orb_set_color(ORB_OFF);
228
                delay_ms(200);
229
                orb_set_color(RED);
230
                delay_ms(200);
231
        }
232
        orb_set_color(ORB_OFF);
233
}
234

    
235

    
236

    
237
int main(void)
238
{
239
        /* Initialize dragonfly board */
240
            dragonfly_init(ALL_ON);
241
            /* Initialize the basic wireless library */
242
            wl_basic_init_default();
243
            /* Set the XBee channel to your assigned channel */        /* Set the XBee channel to your assigned channel */
244
        wl_set_channel(24);
245

    
246
        int robotid = get_robotid();
247
        int used[16];
248
        for (int i=0; i<16; i++) used[i] = 0;
249
        char send_buffer[2];
250
        int data_length;
251
        unsigned char *packet_data=wl_basic_do_default(&data_length);
252
        
253
        
254
        int state = EDGE;
255
        int beacon_State=0;
256
        int waitingCounter=0;
257
        int robotsReceived=0;
258
        if(wheel()<100)
259
        {
260
                state=EDGE;
261
        }
262
        else
263
        {
264
                state=BEACON;
265
        }
266
        
267
        int distance=1000;                                                // how far away to stop.
268
        int onefoot=300, speed=250;                                // one foot is 490 for bot 1; one foot is 200 for bot6
269
        
270
        while(1)
271
        {
272
                switch(state)
273
                {
274
                        case 0:                        // EDGE
275
                                
276
                                bom_off();
277
                                while(1)
278
                                {
279
                                        orb1_set_color(YELLOW);orb2_set_color(CYAN);
280
                                        packet_data=wl_basic_do_default(&data_length);
281
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
282
                                        {
283
                                                send_buffer[0]=CIRCLE_ACTION_ACK;
284
                                                send_buffer[1]=robotid;
285
                                                
286
                                                wl_basic_send_global_packet(42,send_buffer,2);
287
                                                break;
288
                                        }
289
                                }
290
                                
291
                                while(1)                // wait for the center bot to count all the robots and send a "done" packet
292
                                {
293
                                        orb_set_color(YELLOW);orb2_set_color(PURPLE);
294
                                        packet_data=wl_basic_do_default(&data_length);
295
                                        wl_basic_send_global_packet(42,send_buffer,2);
296
                                        
297
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
298
                                        {
299
                                                break;
300
                                        }
301
                                }
302
                                
303
                                orb_set_color(MAGENTA);
304
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
305
                                while(correctTurn()){                                
306
                                }
307
                                //range_init();
308
                                
309
                                
310
                                distance = get_distance();
311
                                while (distance>=onefoot || distance==0)
312
                                {
313
                                        if(distance==0)
314
                                                orb_set_color(WHITE);
315
                                        //correctApproach();
316
                                        distance = get_distance();
317
                                        delay_ms(14);
318
                                }
319

    
320
                                stop();
321
                                orb_set_color(LIME);
322
                                //button1_wait ();                        // code for lab1.
323
                                //go_straight_onefoot();
324
                                break;
325
                                
326
                                
327
                                
328
                                
329
                        case 1:                        // BEACON
330
                                switch(beacon_State) {
331
                                case 0:                // wait for a button to be pressed, then go to state 1
332
                                        bom_on();
333
                                        orb_set_color(PURPLE);
334
                                        if(button1_click()) beacon_State=1;
335
                                break;        
336
                                case 1:                // sends a global exist packet to see how many robots there are
337
                                        orb_set_color(RED);
338
                                        send_buffer[0]=CIRCLE_ACTION_EXIST;
339
                                        send_buffer[1]=get_robotid();
340
                                        wl_basic_send_global_packet(42,send_buffer,2);
341
                                        beacon_State=2;
342
                                break;
343
                                case 2:         // find out how many robots there are
344
                                waitingCounter++;
345
                                orb1_set_color(YELLOW);
346
                                orb2_set_color(BLUE);
347
                                packet_data=wl_basic_do_default(&data_length);
348
                                if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
349
                                {
350
                                        orb_set_color(RED);orb2_set_color(BLUE);
351
                                        if(used[packet_data[1]]==0){                //only add to robots seen if you haven't gotten an ACK from this robot
352
                                                robotsReceived++;
353
                                                used[packet_data[1]]=1;
354
                                        }
355
                                }
356
                                if(waitingCounter >= 1500){
357
                                        beacon_State=3;
358
                                        //blink(robotsReceived);
359
                                }
360
                                break;
361
                                case 3:
362
                                orb_set_color(GREEN);
363
                                send_buffer[0]=CIRCLE_ACTION_DONE;
364
                                wl_basic_send_global_packet(42,send_buffer,2);
365
                                break;
366
                                }
367
                        break;
368
                }
369
        }
370

    
371
        orb_set_color(RED);
372
        while(1); /* END HERE */
373

    
374
        return 0;
375
}