Revision 1626
Merged circle code and added comments
trunk/code/behaviors/formation_control/circle/circle.c | ||
---|---|---|
6 | 6 |
int EDGE = 0; |
7 | 7 |
int BEACON = 1; |
8 | 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; |
|
9 | 14 |
|
10 | 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 |
|
|
11 | 51 |
/* |
12 | 52 |
This program is used to make robots target a center (leader) robot using the BOM, |
13 | 53 |
then drive toward it and stop at a certain distance away. |
... | ... | |
83 | 123 |
} |
84 | 124 |
|
85 | 125 |
|
126 |
|
|
127 |
/*~~~~~~~~~~~~~ NEED document |
|
128 |
*/ |
|
86 | 129 |
int correctTurn(void) |
87 | 130 |
{ |
88 | 131 |
int bomNum = 0; |
... | ... | |
91 | 134 |
usb_puti(bomNum); |
92 | 135 |
if(bomNum == 4) |
93 | 136 |
{ timeout = 0; |
94 |
motor_l_set(1, 200); |
|
95 |
motor_r_set(1, 200); |
|
96 |
return 0;
|
|
137 |
//motor_l_set(1, 200);
|
|
138 |
//motor_r_set(1, 200);
|
|
139 |
return 1; //<--------------------------------------------1 and 0 are switched
|
|
97 | 140 |
} |
98 | 141 |
else |
99 | 142 |
{ |
... | ... | |
116 | 159 |
motor_r_set(FORWARD, 200); |
117 | 160 |
} |
118 | 161 |
} |
119 |
return 1;
|
|
162 |
return 0;
|
|
120 | 163 |
} |
121 | 164 |
|
165 |
|
|
166 |
/*~~~~~~~~~~~~~~ NEED document |
|
167 |
*/ |
|
122 | 168 |
void correctApproach(void) |
123 | 169 |
{ |
124 | 170 |
int bomNum = 0; |
... | ... | |
147 | 193 |
} |
148 | 194 |
} |
149 | 195 |
|
150 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
|
196 |
|
|
197 |
/* |
|
198 |
Start turning until the front receive the MAx bom reading |
|
199 |
*/ |
|
151 | 200 |
void turn_to_beacon(int max){ |
152 | 201 |
if (max>-1 && max<16){ |
153 | 202 |
int index = (max+12)%16; |
... | ... | |
159 | 208 |
} |
160 | 209 |
} |
161 | 210 |
|
211 |
|
|
212 |
/* |
|
213 |
Start turning its fron to the MAx bom reading |
|
214 |
But it won't stop by itself. |
|
215 |
*/ |
|
162 | 216 |
void turn_to_beacon2(int max){ // like the previous but no stop() call' |
163 | 217 |
|
164 | 218 |
|
... | ... | |
174 | 228 |
} |
175 | 229 |
} |
176 | 230 |
|
231 |
|
|
232 |
|
|
233 |
|
|
234 |
/* |
|
235 |
Turn towards the MAX bom direction |
|
236 |
until the front receive the MAx bom reading |
|
237 |
*/ |
|
177 | 238 |
void orient(void){ |
178 | 239 |
int max_index = -1; |
179 | 240 |
while (max_index!=4) { |
... | ... | |
184 | 245 |
delay_ms(22); |
185 | 246 |
} |
186 | 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 |
*/ |
|
187 | 256 |
void orient2(void){ |
188 | 257 |
int max_index = -1; |
189 | 258 |
while (max_index!=4) { |
... | ... | |
217 | 286 |
count++; |
218 | 287 |
} |
219 | 288 |
} |
220 |
void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
|
221 |
go_straight(); |
|
222 |
} |
|
223 | 289 |
|
290 |
|
|
291 |
/* |
|
292 |
BLINK the given number times |
|
293 |
*/ |
|
224 | 294 |
void blink(int num) { |
225 | 295 |
for(int i = 0; i<num; i++) |
226 | 296 |
{ |
... | ... | |
234 | 304 |
|
235 | 305 |
|
236 | 306 |
|
307 |
|
|
308 |
|
|
237 | 309 |
int main(void) |
238 | 310 |
{ |
239 | 311 |
/* Initialize dragonfly board */ |
... | ... | |
243 | 315 |
/* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ |
244 | 316 |
wl_set_channel(24); |
245 | 317 |
|
318 |
int sending_counter = 0; |
|
246 | 319 |
int robotid = get_robotid(); |
247 | 320 |
int used[16]; |
248 | 321 |
for (int i=0; i<16; i++) used[i] = 0; |
... | ... | |
269 | 342 |
|
270 | 343 |
while(1) |
271 | 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. |
|
272 | 365 |
switch(state) |
273 | 366 |
{ |
274 |
case 0: // EDGE |
|
367 |
/********** |
|
368 |
if EDGE /slave robots |
|
369 |
*/ |
|
370 |
case 0: |
|
275 | 371 |
|
276 |
bom_off(); |
|
277 |
while(1) |
|
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) |
|
278 | 379 |
{ |
380 |
bom_off(); |
|
279 | 381 |
orb1_set_color(YELLOW);orb2_set_color(CYAN); |
280 | 382 |
packet_data=wl_basic_do_default(&data_length); |
281 | 383 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
... | ... | |
288 | 390 |
} |
289 | 391 |
} |
290 | 392 |
|
291 |
while(1) // wait for the center bot to count all the robots and send a "done" packet |
|
393 |
/* |
|
394 |
1. Wait for DONE package |
|
395 |
2. The counting process is DONE |
|
396 |
*/ |
|
397 |
while(1) |
|
292 | 398 |
{ |
293 | 399 |
orb_set_color(YELLOW);orb2_set_color(PURPLE); |
294 | 400 |
packet_data=wl_basic_do_default(&data_length); |
... | ... | |
300 | 406 |
} |
301 | 407 |
} |
302 | 408 |
|
409 |
|
|
410 |
// COLOR afer DONE ---> MAGENTA |
|
303 | 411 |
orb_set_color(MAGENTA); |
304 | 412 |
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ |
305 | 413 |
while(correctTurn()){ |
306 | 414 |
} |
415 |
forward(220); |
|
307 | 416 |
//range_init(); |
308 | 417 |
|
309 | 418 |
|
... | ... | |
319 | 428 |
|
320 | 429 |
stop(); |
321 | 430 |
orb_set_color(LIME); |
322 |
//button1_wait (); // code for lab1. |
|
323 |
//go_straight_onefoot(); |
|
324 |
break; |
|
325 | 431 |
|
432 |
send_buffer[0]=CIRCLE_ACTION_ACK; |
|
433 |
send_buffer[1]=robotid; |
|
434 |
wl_basic_send_global_packet(42,send_buffer,2); |
|
326 | 435 |
|
436 |
// NEW CODE FROM JOEL |
|
327 | 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 |
} |
|
328 | 449 |
|
329 |
case 1: // BEACON |
|
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 |
|
330 | 507 |
switch(beacon_State) { |
331 |
case 0: // wait for a button to be pressed, then go to state 1 |
|
508 |
/* |
|
509 |
1. center robots on wait for pressing button 1 |
|
510 |
*/ |
|
511 |
case 0: |
|
332 | 512 |
bom_on(); |
333 | 513 |
orb_set_color(PURPLE); |
334 | 514 |
if(button1_click()) beacon_State=1; |
335 |
break; |
|
515 |
break; |
|
516 |
/* |
|
517 |
1. Send EXIST package to EDGE robots |
|
518 |
*/ |
|
336 | 519 |
case 1: // sends a global exist packet to see how many robots there are |
337 | 520 |
orb_set_color(RED); |
338 | 521 |
send_buffer[0]=CIRCLE_ACTION_EXIST; |
339 | 522 |
send_buffer[1]=get_robotid(); |
340 | 523 |
wl_basic_send_global_packet(42,send_buffer,2); |
341 | 524 |
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; |
|
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 |
} |
|
354 | 543 |
} |
355 |
} |
|
356 |
if(waitingCounter >= 1500){ |
|
357 |
beacon_State=3; |
|
358 |
//blink(robotsReceived); |
|
359 |
} |
|
360 |
break; |
|
544 |
if(waitingCounter >= 1500){ |
|
545 |
beacon_State=3; |
|
546 |
} |
|
547 |
break; |
|
548 |
/* |
|
549 |
COUNTing is DONE. |
|
550 |
Sending DONE package. |
|
551 |
*/ |
|
361 | 552 |
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; |
|
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 |
} |
|
368 | 623 |
} |
369 | 624 |
} |
370 | 625 |
|
371 | 626 |
orb_set_color(RED); |
372 | 627 |
while(1); /* END HERE */ |
373 | 628 |
|
374 |
return 0; |
|
629 |
//return 0;
|
|
375 | 630 |
} |
trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c | ||
---|---|---|
87 | 87 |
if(master) |
88 | 88 |
{ |
89 | 89 |
|
90 |
if(sending_counter++>4) |
|
90 |
if(sending_counter++>4) // clock for switching the BOMs between the master and slave
|
|
91 | 91 |
{ |
92 | 92 |
switch_sending(); |
93 | 93 |
sending_counter = 0; |
... | ... | |
151 | 151 |
else |
152 | 152 |
{ |
153 | 153 |
int max_bom = bom_get_max(); |
154 |
usb_puts("bom_get_max : "); |
|
154 |
/*usb_puts("bom_get_max : ");
|
|
155 | 155 |
usb_puti(max_bom); |
156 |
usb_puts("/n/r"); |
|
156 |
usb_puts("/n/r");*/
|
|
157 | 157 |
|
158 | 158 |
|
159 | 159 |
if(max_bom == 8) |
Also available in: Unified diff