Revision 1800
Robots still oscillate while facing in wrong direction
trunk/code/behaviors/formation_control/circle/circle.c | ||
---|---|---|
207 | 207 |
{ |
208 | 208 |
int inverse = (goal + 8) % 16; // the inverse of the goal direction, across the BOM. |
209 | 209 |
int bomNum = -1; |
210 |
int speed; // speed with which to turn |
|
210 | 211 |
|
211 | 212 |
orb1_set_color(BLUE); // BLUE and PURPLE |
212 |
left(220);
|
|
213 |
left(180);
|
|
213 | 214 |
while(bomNum != goal) |
214 | 215 |
{ |
215 | 216 |
// bomNum is the current maximum reading |
... | ... | |
228 | 229 |
else if (goal > inverse) |
229 | 230 |
{ |
230 | 231 |
if (bomNum < goal && bomNum > inverse) // goal clockwise of position |
231 |
right(220); |
|
232 |
{ |
|
233 |
speed = 160 + 10 * (goal - bomNum); |
|
234 |
right(180); |
|
235 |
} |
|
232 | 236 |
else // goal counterclockwise of position |
233 |
left(220); |
|
237 |
{ |
|
238 |
speed = 160 + 10 * ((bomNum > goal) ? bomNum - goal : goal - bomNum); |
|
239 |
left(180); |
|
240 |
} |
|
234 | 241 |
timeout = 0; |
235 | 242 |
} |
236 | 243 |
else // goal < inverse |
237 | 244 |
{ |
238 | 245 |
if (bomNum > goal && bomNum < inverse) // goal counterclockwise of position |
239 |
left(220); |
|
246 |
{ |
|
247 |
speed = 160 + 10 * (bomNum - goal); |
|
248 |
left(180); |
|
249 |
} |
|
240 | 250 |
else |
241 |
right(220); |
|
251 |
{ |
|
252 |
speed = 160 + 10 * ((bomNum < goal) ? goal - bomNum : bomNum - goal); |
|
253 |
right(180); |
|
254 |
} |
|
242 | 255 |
timeout = 0; |
243 | 256 |
} |
244 | 257 |
} |
Also available in: Unified diff