Revision 1796
| trunk/code/behaviors/formation_control/circle/circle.c (revision 1796) | ||
|---|---|---|
| 205 | 205 |
*/ |
| 206 | 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; |
|
| 208 |
int goala = goal; |
|
| 209 |
int goalb = goal + 8; // the inverse of the goal direction, across the BOM. |
|
| 210 |
if(goalb >= 16) { goalb -= 16; }
|
|
| 210 | 211 |
|
| 212 |
int inv = 0; |
|
| 213 |
if(goala > goalb) |
|
| 214 |
{
|
|
| 215 |
goala = goalb; |
|
| 216 |
goalb = goal; |
|
| 217 |
inv = 1; |
|
| 218 |
} |
|
| 219 |
|
|
| 211 | 220 |
orb1_set_color(BLUE); // BLUE and PURPLE |
| 212 | 221 |
left(220); |
| 213 | 222 |
while(1) |
| 214 | 223 |
{
|
| 215 |
int bomNum = 0; // bomNum is the current maximum reading |
|
| 224 |
// bomNum is the current maximum reading |
|
| 216 | 225 |
bom_refresh(BOM_ALL); |
| 217 |
bomNum = bom_get_max(); |
|
| 218 |
usb_puti(bomNum); |
|
| 226 |
int bomNum = bom_get_max(); |
|
| 219 | 227 |
if(bomNum == goal) // when it's turned the right way, stop |
| 220 | 228 |
{
|
| 221 | 229 |
timeout = 0; |
| 222 |
//motor_l_set(1, 200); |
|
| 223 |
//motor_r_set(1, 200); |
|
| 230 |
stop(); |
|
| 224 | 231 |
break; // exits the while() loop to stop the method |
| 225 | 232 |
} |
| 226 | 233 |
else // facing the wrong way |
| 227 | 234 |
{
|
| 235 |
|
|
| 228 | 236 |
if(bomNum == -1) |
| 229 | 237 |
{
|
| 230 | 238 |
timeout++; |
| ... | ... | |
| 235 | 243 |
motor_l_set(BACKWARD, 190); |
| 236 | 244 |
} |
| 237 | 245 |
} |
| 238 |
else if((bomNum >= goalinv) || (bomNum < goal)) |
|
| 246 |
else if((bomNum >= goalb) || (bomNum < goala)) |
|
| 239 | 247 |
{
|
| 240 |
motor_l_set(FORWARD, 200); |
|
| 241 |
motor_r_set(BACKWARD, 200); |
|
| 248 |
if(inv) |
|
| 249 |
right(200); |
|
| 250 |
else |
|
| 251 |
left(200); |
|
| 242 | 252 |
timeout = 0; |
| 243 | 253 |
} |
| 244 | 254 |
else |
| 245 | 255 |
{
|
| 246 |
motor_l_set(BACKWARD, 200); |
|
| 247 |
motor_r_set(FORWARD, 200); |
|
| 256 |
if(inv) |
|
| 257 |
left(200); |
|
| 258 |
else |
|
| 259 |
right(200); |
|
| 248 | 260 |
timeout = 0; |
| 249 | 261 |
} |
| 250 | 262 |
} |
| ... | ... | |
| 608 | 620 |
{
|
| 609 | 621 |
bom_off(); |
| 610 | 622 |
direction = packet_data[2]; |
| 611 |
blink(direction); |
|
| 612 | 623 |
orb_set_color(YELLOW); |
| 613 | 624 |
edge_State = 2; |
| 614 | 625 |
} |
| ... | ... | |
| 629 | 640 |
/* Turn until we reach the right direction (DIRECTION) */ |
| 630 | 641 |
case 3: |
| 631 | 642 |
aboutFace(direction); |
| 632 |
|
|
| 633 |
/*left(170); |
|
| 634 |
bom_refresh(BOM_ALL); |
|
| 635 |
if(bom_get_max() == direction) |
|
| 636 |
{
|
|
| 637 |
stop(); |
|
| 638 |
orb_set_color(YELLOW); |
|
| 639 |
state = 1; |
|
| 640 |
}*/ |
|
| 641 | 643 |
break; |
| 642 | 644 |
|
| 643 | 645 |
} |
| ... | ... | |
| 816 | 818 |
direction = bom_get_max(); |
| 817 | 819 |
direction += 8; |
| 818 | 820 |
if(direction > 15) direction -= 16; |
| 819 |
blink(direction); |
|
| 820 | 821 |
send3(CIRCLE_ACTION_GOTYOU, i, direction); |
| 821 | 822 |
delay_ms(20); |
| 822 | 823 |
} |
Also available in: Unified diff