Revision 1776 trunk/code/behaviors/formation_control/circle/circle.c
| circle.c (revision 1776) | ||
|---|---|---|
| 203 | 203 |
Orients the robot so that it is facing the beacon (or the broadcasting BOM). |
| 204 | 204 |
|
| 205 | 205 |
*/ |
| 206 |
void correctTurn(void) |
|
| 206 |
void aboutFace(int goal) |
|
| 207 | 207 |
{
|
| 208 |
int goalinv = goal + 8; // the inverse of the goal direction, across the BOM. |
|
| 209 |
if(goalinv >= 16) goalinv -= 16; |
|
| 210 |
|
|
| 208 | 211 |
orb1_set_color(BLUE); // BLUE and PURPLE |
| 209 | 212 |
left(220); |
| 210 | 213 |
while(1) |
| ... | ... | |
| 213 | 216 |
bom_refresh(BOM_ALL); |
| 214 | 217 |
bomNum = bom_get_max(); |
| 215 | 218 |
usb_puti(bomNum); |
| 216 |
if(bomNum == 4) // when it's turned the right way, stop |
|
| 219 |
if(bomNum == goal) // when it's turned the right way, stop |
|
| 217 | 220 |
{
|
| 218 | 221 |
timeout = 0; |
| 219 | 222 |
//motor_l_set(1, 200); |
| ... | ... | |
| 232 | 235 |
motor_l_set(BACKWARD, 190); |
| 233 | 236 |
} |
| 234 | 237 |
} |
| 235 |
else if((bomNum >= 12) || (bomNum < 4)) |
|
| 238 |
else if((bomNum >= goalinv) || (bomNum < goal)) |
|
| 236 | 239 |
{
|
| 237 | 240 |
motor_l_set(FORWARD, 200); |
| 238 | 241 |
motor_r_set(BACKWARD, 200); |
| ... | ... | |
| 537 | 540 |
case 0: |
| 538 | 541 |
// COLOR afer DONE ---> MAGENTA |
| 539 | 542 |
orb_set_color(MAGENTA); |
| 540 |
correctTurn(); // turn to face the beacon |
|
| 543 |
aboutFace(4); // turn to face the beacon |
|
| 541 | 544 |
forward(175); |
| 542 | 545 |
//range_init(); |
| 543 | 546 |
|
| ... | ... | |
| 558 | 561 |
time+=14; |
| 559 | 562 |
if(time>50) |
| 560 | 563 |
{
|
| 561 |
correctTurn(); |
|
| 564 |
aboutFace(4); |
|
| 562 | 565 |
time=0; |
| 563 | 566 |
} |
| 564 | 567 |
} |
| ... | ... | |
| 625 | 628 |
|
| 626 | 629 |
/* Turn until we reach the right direction (DIRECTION) */ |
| 627 | 630 |
case 3: |
| 628 |
bom_refresh(BOM_ALL); |
|
| 629 |
int currentdir = bom_get_max(); |
|
| 630 |
int direction2 = direction+8; |
|
| 631 |
|
|
| 632 |
if(currentdir == direction) |
|
| 633 |
{
|
|
| 634 |
stop(); |
|
| 635 |
state = 1; |
|
| 636 |
} |
|
| 637 |
else if( |
|
| 631 |
aboutFace(direction); |
|
| 638 | 632 |
|
| 639 | 633 |
/*left(170); |
| 640 | 634 |
bom_refresh(BOM_ALL); |
Also available in: Unified diff