Revision 1808 trunk/code/behaviors/formation_control/circle/circle.c
| circle.c (revision 1808) | ||
|---|---|---|
| 236 | 236 |
*/ |
| 237 | 237 |
void faceFront(void) |
| 238 | 238 |
{
|
| 239 |
int counter = 0; |
|
| 240 |
int currentDir = 0; |
|
| 241 |
left(200); |
|
| 239 | 242 |
int bomNum = -1; |
| 240 | 243 |
orb1_set_color(BLUE); |
| 241 | 244 |
while(bomNum != 4) |
| 242 | 245 |
{
|
| 246 |
if(counter >= 5) |
|
| 247 |
{
|
|
| 248 |
forward(200); |
|
| 249 |
delay_ms(750); |
|
| 250 |
counter = 0; |
|
| 251 |
} |
|
| 243 | 252 |
bom_refresh(BOM_ALL); |
| 244 | 253 |
bomNum = bom_get_max(); |
| 245 | 254 |
if(bomNum == -1) |
| ... | ... | |
| 249 | 258 |
else if((bomNum < 4) || (bomNum >= 12)) |
| 250 | 259 |
{
|
| 251 | 260 |
right(200); |
| 261 |
if(currentDir == 0) |
|
| 262 |
counter++; |
|
| 263 |
currentDir = 1; |
|
| 252 | 264 |
} |
| 253 | 265 |
else |
| 254 | 266 |
{
|
| 255 | 267 |
left(200); |
| 268 |
if(currentDir == 1) |
|
| 269 |
counter++; |
|
| 270 |
currentDir = 0; |
|
| 256 | 271 |
} |
| 257 | 272 |
} |
| 258 | 273 |
stop(); |
| ... | ... | |
| 544 | 559 |
order(ORIENT); break; |
| 545 | 560 |
|
| 546 | 561 |
case 3: |
| 547 |
orderMove(TURNL,20,2); break; |
|
| 562 |
orderMove(DRIVE,20,2); break; |
|
| 548 | 563 |
|
| 549 | 564 |
case 4: |
| 565 |
order(CIRCLEUP); break; |
|
| 566 |
|
|
| 567 |
case 5: |
|
| 568 |
order(ORIENT); break; |
|
| 569 |
|
|
| 570 |
case 6: |
|
| 571 |
orderMove(TURNR,18,1); break; |
|
| 572 |
|
|
| 573 |
case 7: |
|
| 574 |
orderMove(DRIVE,20,2); break; |
|
| 575 |
|
|
| 576 |
case 8: |
|
| 577 |
order(CIRCLEUP); break; |
|
| 578 |
|
|
| 579 |
case 9: |
|
| 580 |
order(ORIENT); break; |
|
| 581 |
|
|
| 582 |
case 10: |
|
| 550 | 583 |
terminate(); break; |
| 584 |
|
|
| 551 | 585 |
} |
| 552 | 586 |
|
| 553 | 587 |
break; |
| ... | ... | |
| 659 | 693 |
distance = get_distance(); |
| 660 | 694 |
delay_ms(14); |
| 661 | 695 |
time+=14; |
| 662 |
if(time>50) |
|
| 696 |
if(time>30) |
|
| 663 | 697 |
{
|
| 664 | 698 |
faceFront(); |
| 665 | 699 |
time=0; |
| ... | ... | |
| 747 | 781 |
|
| 748 | 782 |
|
| 749 | 783 |
/* The DRIVE steps for the EDGE robot */ |
| 750 |
|
|
| 751 | 784 |
case 13: |
| 752 |
|
|
| 753 |
switch(edge_State) |
|
| 785 |
|
|
| 786 |
/* Wait for specifications to drive */ |
|
| 787 |
packet_data=wl_basic_do_default(&data_length); |
|
| 788 |
if(packet_data != 0 && data_length>=3 && |
|
| 789 |
packet_data[0]==CIRCLE_ACTION_FORWARD) |
|
| 754 | 790 |
{
|
| 755 |
|
|
| 756 |
/* Wait for the command to move forward. */ |
|
| 757 |
case 0: |
|
| 758 |
packet_data=wl_basic_do_default(&data_length); |
|
| 759 |
if(packet_data != 0 && data_length>=3 && |
|
| 760 |
packet_data[0]==CIRCLE_ACTION_FORWARD) |
|
| 761 |
{
|
|
| 762 |
orb_set_color(BLUE); |
|
| 763 |
forward(packet_data[1]*10); |
|
| 764 |
delay_ms(packet_data[2]*1000); |
|
| 765 |
stop(); |
|
| 766 |
state = 1; |
|
| 767 |
} |
|
| 768 |
break; |
|
| 791 |
orb_set_color(BLUE); |
|
| 769 | 792 |
|
| 770 |
|
|
| 793 |
forward(packet_data[1]*10); |
|
| 794 |
delay_ms(packet_data[2]*1000); |
|
| 795 |
stop(); |
|
| 796 |
state = 1; |
|
| 771 | 797 |
} |
| 772 | 798 |
|
| 773 | 799 |
break; |
| 774 | 800 |
|
| 775 | 801 |
/* The TURNL steps for the EDGE robot */ |
| 776 | 802 |
case 14: |
| 777 |
orb_set_color(RED); |
|
| 778 | 803 |
|
| 779 | 804 |
/* Wait for specifications for the turn. */ |
| 780 |
|
|
| 781 | 805 |
packet_data=wl_basic_do_default(&data_length); |
| 782 | 806 |
if(packet_data != 0 && data_length>=3 && |
| 783 | 807 |
packet_data[0]==CIRCLE_ACTION_TURN) |
| ... | ... | |
| 793 | 817 |
|
| 794 | 818 |
/* The TURNR steps for the EDGE robot */ |
| 795 | 819 |
case 15: |
| 796 |
orb_set_color(RED); |
|
| 797 | 820 |
|
| 798 | 821 |
/* Wait for specifications for the turn. */ |
| 799 |
|
|
| 800 | 822 |
packet_data=wl_basic_do_default(&data_length); |
| 801 | 823 |
if(packet_data != 0 && data_length>=3 && |
| 802 | 824 |
packet_data[0]==CIRCLE_ACTION_TURN) |
| ... | ... | |
| 897 | 919 |
|
| 898 | 920 |
/* Wait for all the robots to get to right distance */ |
| 899 | 921 |
case 0: |
| 900 |
left(170); |
|
| 922 |
// left(170); |
|
| 901 | 923 |
orb1_set_color(YELLOW); |
| 902 | 924 |
orb2_set_color(WHITE); |
| 903 | 925 |
|
| ... | ... | |
| 1009 | 1031 |
/* The DRIVE code for the beacon */ |
| 1010 | 1032 |
case 23: |
| 1011 | 1033 |
|
| 1012 |
switch(beacon_State) |
|
| 1013 |
{
|
|
| 1034 |
orb_set_color(YELLOW); |
|
| 1035 |
delay_ms(100); |
|
| 1014 | 1036 |
|
| 1015 |
/* Tells the robots to move forward and moves itself. */ |
|
| 1016 |
case 0: |
|
| 1017 |
orb_set_color(YELLOW); |
|
| 1018 |
delay_ms(50); |
|
| 1019 |
|
|
| 1020 |
// format: type of ack, speed divided by 10, |
|
| 1021 |
// time in seconds. |
|
| 1037 |
// format: type of ack, speed divided by 10, |
|
| 1038 |
// time in seconds. |
|
| 1039 |
for(int i = 0 ; i < 13; i++) |
|
| 1022 | 1040 |
send3(CIRCLE_ACTION_FORWARD,speed,duration); |
| 1023 |
orb_set_color(BLUE); |
|
| 1024 |
forward(speed*10); |
|
| 1025 |
delay_ms(duration*1000); |
|
| 1026 |
stop(); |
|
| 1027 |
beacon_State = 1; |
|
| 1028 |
break; |
|
| 1029 |
|
|
| 1030 |
/* Terminal. */ |
|
| 1031 |
case 1: |
|
| 1032 |
stop(); |
|
| 1033 |
state = 2; |
|
| 1034 |
break; |
|
| 1035 |
} |
|
| 1041 |
orb_set_color(BLUE); |
|
| 1042 |
forward(speed*10); |
|
| 1043 |
delay_ms(duration*1000); |
|
| 1044 |
stop(); |
|
| 1045 |
state = 2; |
|
| 1036 | 1046 |
break; |
| 1037 | 1047 |
|
| 1038 | 1048 |
/* The TURNL code for the beacon */ |
| 1039 | 1049 |
case 24: |
| 1040 | 1050 |
|
| 1041 | 1051 |
orb_set_color(YELLOW); |
| 1042 |
delay_ms(50); |
|
| 1052 |
delay_ms(100); |
|
| 1043 | 1053 |
|
| 1044 | 1054 |
// format: type of ack, speed divided by 10, |
| 1045 | 1055 |
// time in seconds. |
| 1046 |
for(int i = 0 ; i < 5; i++) |
|
| 1056 |
for(int i = 0 ; i < 13; i++) |
|
| 1047 | 1057 |
send3(CIRCLE_ACTION_TURN,speed,duration); |
| 1048 | 1058 |
orb_set_color(BLUE); |
| 1049 | 1059 |
left(speed*10); |
| ... | ... | |
| 1057 | 1067 |
case 25: |
| 1058 | 1068 |
|
| 1059 | 1069 |
orb_set_color(YELLOW); |
| 1060 |
delay_ms(50); |
|
| 1070 |
delay_ms(100); |
|
| 1061 | 1071 |
|
| 1062 | 1072 |
// format: type of ack, speed divided by 10, |
| 1063 | 1073 |
// time in seconds. |
| 1064 |
for(int i = 0 ; i < 5; i++) |
|
| 1074 |
for(int i = 0 ; i < 13; i++) |
|
| 1065 | 1075 |
send3(CIRCLE_ACTION_TURN,speed,duration); |
| 1066 | 1076 |
orb_set_color(BLUE); |
| 1067 | 1077 |
right(speed*10); |
Also available in: Unified diff