Revision 1775
Fixed the code so it works again with the restructuring; working on a better turning algorithm.
| circle.c | ||
|---|---|---|
| 369 | 369 |
{
|
| 370 | 370 |
centerid = packet_data[1]; |
| 371 | 371 |
|
| 372 |
send2(CIRCLE_ACTION_ACK,robotid); |
|
| 373 |
|
|
| 374 |
state = EDGE_CONTROL; |
|
| 372 |
state = 1; |
|
| 375 | 373 |
} |
| 376 | 374 |
|
| 377 | 375 |
if(button1_read()) |
| 378 | 376 |
{
|
| 379 | 377 |
send2(CIRCLE_CLAIM_CENTER, robotid); // becomes the center if button1 is clicked. |
| 378 |
state = 2; |
|
| 380 | 379 |
} |
| 381 | 380 |
|
| 382 | 381 |
break; |
| ... | ... | |
| 406 | 405 |
|
| 407 | 406 |
if(command != -1) |
| 408 | 407 |
{
|
| 408 |
edge_State = 0; |
|
| 409 | 409 |
switch(command) |
| 410 | 410 |
{
|
| 411 | 411 |
case 0: |
| ... | ... | |
| 439 | 439 |
*/ |
| 440 | 440 |
case 2: |
| 441 | 441 |
orb_set_color(PURPLE); |
| 442 |
|
|
| 442 |
beacon_State = 0; |
|
| 443 | 443 |
|
| 444 | 444 |
switch(currentPos) |
| 445 | 445 |
{
|
| ... | ... | |
| 474 | 474 |
case 10: |
| 475 | 475 |
|
| 476 | 476 |
|
| 477 |
edge_State = 0; |
|
| 478 |
|
|
| 479 | 477 |
switch(edge_State) |
| 480 | 478 |
{
|
| 481 | 479 |
/* |
| ... | ... | |
| 487 | 485 |
case 0: |
| 488 | 486 |
bom_off(); |
| 489 | 487 |
orb1_set_color(YELLOW); |
| 490 |
orb2_set_color(CYAN);
|
|
| 488 |
orb2_set_color(BLUE);
|
|
| 491 | 489 |
packet_data=wl_basic_do_default(&data_length); |
| 492 | 490 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
| 493 | 491 |
{
|
| ... | ... | |
| 533 | 531 |
|
| 534 | 532 |
case 11: |
| 535 | 533 |
|
| 536 |
edge_State = 0; |
|
| 537 | 534 |
switch(edge_State) |
| 538 | 535 |
{
|
| 539 | 536 |
|
| ... | ... | |
| 584 | 581 |
|
| 585 | 582 |
case 12: |
| 586 | 583 |
|
| 587 |
edge_State = 0; |
|
| 588 | 584 |
|
| 589 | 585 |
switch(edge_State) |
| 590 | 586 |
{
|
| ... | ... | |
| 609 | 605 |
{
|
| 610 | 606 |
bom_off(); |
| 611 | 607 |
direction = packet_data[2]; |
| 608 |
blink(direction); |
|
| 612 | 609 |
orb_set_color(YELLOW); |
| 613 | 610 |
edge_State = 2; |
| 614 | 611 |
} |
| ... | ... | |
| 628 | 625 |
|
| 629 | 626 |
/* Turn until we reach the right direction (DIRECTION) */ |
| 630 | 627 |
case 3: |
| 631 |
left(170); |
|
| 632 | 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( |
|
| 638 |
|
|
| 639 |
/*left(170); |
|
| 640 |
bom_refresh(BOM_ALL); |
|
| 633 | 641 |
if(bom_get_max() == direction) |
| 634 | 642 |
{
|
| 635 | 643 |
stop(); |
| 636 | 644 |
orb_set_color(YELLOW); |
| 637 | 645 |
state = 1; |
| 638 |
} |
|
| 646 |
}*/
|
|
| 639 | 647 |
break; |
| 640 | 648 |
|
| 641 | 649 |
} |
| ... | ... | |
| 648 | 656 |
|
| 649 | 657 |
case 13: |
| 650 | 658 |
|
| 651 |
edge_State = 0; |
|
| 652 | 659 |
switch(edge_State) |
| 653 | 660 |
{
|
| 654 | 661 |
|
| ... | ... | |
| 658 | 665 |
if(packet_data != 0 && data_length>=3 && packet_data[0]==CIRCLE_ACTION_FORWARD) |
| 659 | 666 |
{
|
| 660 | 667 |
orb_set_color(BLUE); |
| 661 |
forward(200);
|
|
| 662 |
delay_ms(2000);
|
|
| 668 |
forward(packet_data[1]*10);
|
|
| 669 |
delay_ms(packet_data[2]*1000);
|
|
| 663 | 670 |
edge_State = 1; |
| 664 | 671 |
} |
| 665 | 672 |
break; |
| ... | ... | |
| 693 | 700 |
|
| 694 | 701 |
/* the COUNT code for the BEACON */ |
| 695 | 702 |
case 20: |
| 696 |
beacon_State = 0; |
|
| 697 | 703 |
switch(beacon_State) |
| 698 | 704 |
{
|
| 699 | 705 |
|
| 700 | 706 |
/* 0. center robots on wait for pressing button 1 */ |
| 701 | 707 |
case 0: |
| 702 | 708 |
bom_on(); |
| 703 |
orb_set_color(PURPLE);
|
|
| 709 |
orb_set_color(BLUE);
|
|
| 704 | 710 |
robotsReceived = 0; |
| 705 |
if(button1_click()) beacon_State=1;
|
|
| 711 |
beacon_State=1; |
|
| 706 | 712 |
break; |
| 707 | 713 |
|
| 708 | 714 |
/* 1. Send EXIST package to EDGE robots */ |
| ... | ... | |
| 756 | 762 |
/* The CIRCLEUP method for BEACON */ |
| 757 | 763 |
case 21: |
| 758 | 764 |
|
| 759 |
beacon_State = 0; |
|
| 760 | 765 |
switch(beacon_State) |
| 761 | 766 |
{
|
| 762 | 767 |
|
| ... | ... | |
| 787 | 792 |
/* The ORIENT code for the beacon */ |
| 788 | 793 |
case 22: |
| 789 | 794 |
|
| 790 |
beacon_State = 0; |
|
| 791 | 795 |
switch(beacon_State) |
| 792 | 796 |
{
|
| 793 | 797 |
/* Turns all the robots in the same direction */ |
| ... | ... | |
| 818 | 822 |
direction = bom_get_max(); |
| 819 | 823 |
direction += 8; |
| 820 | 824 |
if(direction > 15) direction -= 16; |
| 825 |
blink(direction); |
|
| 821 | 826 |
send3(CIRCLE_ACTION_GOTYOU, i, direction); |
| 822 | 827 |
delay_ms(20); |
| 823 | 828 |
} |
| ... | ... | |
| 840 | 845 |
/* The DRIVE code for the beacon */ |
| 841 | 846 |
case 23: |
| 842 | 847 |
|
| 843 |
beacon_State = 0; |
|
| 844 | 848 |
switch(beacon_State) |
| 845 | 849 |
{
|
| 846 | 850 |
|
| 847 | 851 |
/* Tells the robots to move forward and moves itself. */ |
| 848 |
case 2:
|
|
| 852 |
case 0:
|
|
| 849 | 853 |
orb_set_color(YELLOW); |
| 850 | 854 |
delay_ms(5000); |
| 851 | 855 |
|
| 852 |
// format: type of ack, speed, time.
|
|
| 853 |
send3(CIRCLE_ACTION_FORWARD,200,200);
|
|
| 856 |
// format: type of ack, speed divided by 10, time in seconds.
|
|
| 857 |
send3(CIRCLE_ACTION_FORWARD,20,2);
|
|
| 854 | 858 |
orb_set_color(BLUE); |
| 855 | 859 |
forward(200); |
| 856 | 860 |
delay_ms(2000); |
| 857 | 861 |
stop(); |
| 858 |
beacon_State = 3;
|
|
| 862 |
beacon_State = 1;
|
|
| 859 | 863 |
break; |
| 860 | 864 |
|
| 861 | 865 |
/* Terminal. */ |
| 862 |
case 3:
|
|
| 866 |
case 1:
|
|
| 863 | 867 |
stop(); |
| 864 | 868 |
state = 2; |
| 865 | 869 |
break; |
Also available in: Unified diff