Revision 1805
Changed the turning methods to simpler versions which should work more consistently -circle.c
| circle.c | ||
|---|---|---|
| 203 | 203 |
Orients the robot so that it is facing the beacon (or the broadcasting BOM). |
| 204 | 204 |
|
| 205 | 205 |
*/ |
| 206 |
void aboutFace(int goal)
|
|
| 206 |
void faceFront(void)
|
|
| 207 | 207 |
{
|
| 208 |
int inverse = (goal + 8) % 16; // the inverse of the goal direction, across the BOM. |
|
| 209 | 208 |
int bomNum = -1; |
| 210 |
int speed; // speed with which to turn |
|
| 211 |
|
|
| 212 |
orb1_set_color(BLUE); // BLUE and PURPLE |
|
| 213 |
left(180); |
|
| 214 |
while(bomNum != goal) |
|
| 209 |
orb1_set_color(BLUE); |
|
| 210 |
while(bomNum != 4) |
|
| 215 | 211 |
{
|
| 216 |
// bomNum is the current maximum reading |
|
| 217 | 212 |
bom_refresh(BOM_ALL); |
| 218 | 213 |
bomNum = bom_get_max(); |
| 219 |
if(bomNum == -1) // no other robots visible
|
|
| 214 |
if(bomNum == -1) |
|
| 220 | 215 |
{
|
| 221 |
timeout++; |
|
| 222 |
|
|
| 223 |
if(timeout > 50) // if it's been looking too long, move a little bit as it turns |
|
| 224 |
{
|
|
| 225 |
motor_r_set(FORWARD, 210); |
|
| 226 |
motor_l_set(BACKWARD, 190); |
|
| 227 |
} |
|
| 216 |
//ignore |
|
| 228 | 217 |
} |
| 229 |
else if (goal > inverse)
|
|
| 218 |
else if((bomNum < 4) || (bomNum >= 12))
|
|
| 230 | 219 |
{
|
| 231 |
if (bomNum < goal && bomNum > inverse) // goal clockwise of position |
|
| 232 |
{
|
|
| 233 |
speed = 160 + 10 * (goal - bomNum); |
|
| 234 |
right(180); |
|
| 235 |
} |
|
| 236 |
else // goal counterclockwise of position |
|
| 237 |
{
|
|
| 238 |
speed = 160 + 10 * ((bomNum > goal) ? bomNum - goal : goal - bomNum); |
|
| 239 |
left(180); |
|
| 240 |
} |
|
| 241 |
timeout = 0; |
|
| 220 |
right(200); |
|
| 242 | 221 |
} |
| 243 |
else // goal < inverse
|
|
| 222 |
else |
|
| 244 | 223 |
{
|
| 245 |
if (bomNum > goal && bomNum < inverse) // goal counterclockwise of position |
|
| 246 |
{
|
|
| 247 |
speed = 160 + 10 * (bomNum - goal); |
|
| 248 |
left(180); |
|
| 249 |
} |
|
| 250 |
else |
|
| 251 |
{
|
|
| 252 |
speed = 160 + 10 * ((bomNum < goal) ? goal - bomNum : bomNum - goal); |
|
| 253 |
right(180); |
|
| 254 |
} |
|
| 255 |
timeout = 0; |
|
| 224 |
left(200); |
|
| 256 | 225 |
} |
| 257 | 226 |
} |
| 227 |
stop(); |
|
| 258 | 228 |
return; |
| 259 | 229 |
} |
| 260 | 230 |
|
| 231 |
void aboutFace(int goal) |
|
| 232 |
{
|
|
| 233 |
int bomNum = -1; |
|
| 234 |
int speed = 170; // speed with which to turn |
|
| 261 | 235 |
|
| 236 |
orb1_set_color(BLUE); // BLUE and PURPLE |
|
| 237 |
|
|
| 238 |
while(bomNum != goal) |
|
| 239 |
{
|
|
| 240 |
// bomNum is the current maximum reading |
|
| 241 |
bom_refresh(BOM_ALL); |
|
| 242 |
bomNum = bom_get_max(); |
|
| 243 |
right(speed); |
|
| 244 |
} |
|
| 245 |
stop(); |
|
| 246 |
return; |
|
| 247 |
} |
|
| 248 |
|
|
| 249 |
|
|
| 262 | 250 |
/* |
| 263 | 251 |
BLINK the given number times |
| 264 | 252 |
*/ |
| ... | ... | |
| 546 | 534 |
case 0: |
| 547 | 535 |
// COLOR afer DONE ---> MAGENTA |
| 548 | 536 |
orb_set_color(MAGENTA); |
| 549 |
aboutFace(4); // turn to face the beacon
|
|
| 537 |
faceFront(); // turn to face the beacon
|
|
| 550 | 538 |
forward(175); |
| 551 | 539 |
//range_init(); |
| 552 | 540 |
|
| ... | ... | |
| 561 | 549 |
forward(175); |
| 562 | 550 |
else |
| 563 | 551 |
backward(175); |
| 564 |
//correctApproach(); |
|
| 565 | 552 |
distance = get_distance(); |
| 566 | 553 |
delay_ms(14); |
| 567 | 554 |
time+=14; |
| 568 | 555 |
if(time>50) |
| 569 | 556 |
{
|
| 570 |
aboutFace(4);
|
|
| 557 |
faceFront;
|
|
| 571 | 558 |
time=0; |
| 572 | 559 |
} |
| 573 | 560 |
} |
Also available in: Unified diff