Revision 1635
Debugged circle.c so approaching works
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