Revision 1635
Debugged circle.c so approaching works
| trunk/code/behaviors/formation_control/circle/circle.c | ||
|---|---|---|
| 12 | 12 |
int desired_max_bom; |
| 13 | 13 |
int bom_max_counter; |
| 14 | 14 |
|
| 15 |
|
|
| 15 |
//Last used 12,13,7(BOM) |
|
| 16 | 16 |
void set_desired_max_bom(int desired_angle) |
| 17 | 17 |
{
|
| 18 | 18 |
if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0; |
| ... | ... | |
| 269 | 269 |
|
| 270 | 270 |
int state = EDGE; |
| 271 | 271 |
int beacon_State=0; |
| 272 |
int edge_State=0; |
|
| 272 | 273 |
int waitingCounter=0; |
| 273 | 274 |
int robotsReceived=0; |
| 275 |
int offset = 20, time=0; |
|
| 274 | 276 |
|
| 275 | 277 |
if(wheel()<100) |
| 276 | 278 |
{
|
| ... | ... | |
| 282 | 284 |
} |
| 283 | 285 |
|
| 284 | 286 |
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
|
|
| 287 |
int onefoot=250, speed=250; // one foot is 490 for bot 1; one foot is 200 for bot6
|
|
| 286 | 288 |
|
| 287 | 289 |
while(1) |
| 288 | 290 |
{
|
| ... | ... | |
| 312 | 314 |
if EDGE /slave robots |
| 313 | 315 |
*/ |
| 314 | 316 |
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) |
|
| 317 |
switch(edge_State) |
|
| 323 | 318 |
{
|
| 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; |
|
| 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); |
|
| 331 | 347 |
|
| 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); |
|
| 348 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) |
|
| 349 |
{
|
|
| 350 |
edge_State=2; |
|
| 351 |
} |
|
| 352 |
break; |
|
| 346 | 353 |
|
| 347 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) |
|
| 348 |
{
|
|
| 349 |
break; |
|
| 350 |
} |
|
| 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; |
|
| 351 | 390 |
} |
| 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; |
|
| 391 |
/*send_buffer[0]=CIRCLE_ACTION_ACK; |
|
| 376 | 392 |
send_buffer[1]=robotid; |
| 377 | 393 |
wl_basic_send_global_packet(42,send_buffer,2); |
| 378 | 394 |
|
| ... | ... | |
| 406 | 422 |
/*usb_puts("bom_get_max : ");
|
| 407 | 423 |
usb_puti(max_bom); |
| 408 | 424 |
usb_puts("/n/r");*/
|
| 425 |
/* |
|
| 409 | 426 |
|
| 410 |
|
|
| 411 | 427 |
if(max_bom == 8) |
| 412 | 428 |
{
|
| 413 | 429 |
orb2_set_color(BLUE); |
| ... | ... | |
| 434 | 450 |
delay_ms(10); |
| 435 | 451 |
|
| 436 | 452 |
} //end while |
| 437 |
break; |
|
| 453 |
*/break;
|
|
| 438 | 454 |
|
| 439 | 455 |
// END for EDGE robots |
| 440 | 456 |
|
| ... | ... | |
| 449 | 465 |
case 1: // BEACON /master /enter robots |
| 450 | 466 |
switch(beacon_State) {
|
| 451 | 467 |
/* |
| 452 |
1. center robots on wait for pressing button 1
|
|
| 468 |
0. center robots on wait for pressing button 1
|
|
| 453 | 469 |
*/ |
| 454 | 470 |
case 0: |
| 455 | 471 |
bom_on(); |
| ... | ... | |
| 467 | 483 |
beacon_State=2; |
| 468 | 484 |
break; |
| 469 | 485 |
/* |
| 470 |
1. Count the number of the EDGE robots
|
|
| 486 |
2. Count the number of the EDGE robots
|
|
| 471 | 487 |
*******NOTE: at most 1500 times of loop ****** |
| 472 | 488 |
*/ |
| 473 | 489 |
case 2: |
| ... | ... | |
| 485 | 501 |
used[packet_data[1]]=1; |
| 486 | 502 |
} |
| 487 | 503 |
} |
| 488 |
if(waitingCounter >= 1500){
|
|
| 504 |
if(waitingCounter >= 750){
|
|
| 489 | 505 |
beacon_State=3; |
| 490 | 506 |
} |
| 491 | 507 |
break; |
| ... | ... | |
| 504 | 520 |
Wait for all the robots to get to right distance/position |
| 505 | 521 |
*/ |
| 506 | 522 |
case 4: |
| 507 |
orb1_set_color(YELLOW); |
|
| 523 |
/*orb1_set_color(YELLOW);
|
|
| 508 | 524 |
orb2_set_color(WHITE); |
| 509 | 525 |
|
| 510 | 526 |
int numOk = 0; |
| ... | ... | |
| 518 | 534 |
} |
| 519 | 535 |
} |
| 520 | 536 |
|
| 521 |
beacon_State = 5; |
|
| 537 |
beacon_State = 5;*/
|
|
| 522 | 538 |
break; // <----------------------------------------------------------Echo wrote this "break". need to be checked. |
| 523 | 539 |
|
| 524 | 540 |
/** |
| 525 | 541 |
NEED to be documented |
| 526 | 542 |
*/ |
| 527 | 543 |
case 5: |
| 528 |
orb_set_color(BLUE); |
|
| 544 |
/*orb_set_color(BLUE);
|
|
| 529 | 545 |
// clock for switching the BOMs between the master and slave |
| 530 | 546 |
if(sending_counter++>4) |
| 531 | 547 |
{
|
| ... | ... | |
| 562 | 578 |
else bom_max_counter = 0; |
| 563 | 579 |
|
| 564 | 580 |
} |
| 565 |
|
|
| 581 |
*/ |
|
| 566 | 582 |
break; |
| 567 | 583 |
} |
| 568 | 584 |
} |
Also available in: Unified diff