Revision 591
BFS worked in 2 out of many trials with 2 robots. However, it dropped wireless a lot and changes states more than it should.
| smart_run_around_fsm.c | ||
|---|---|---|
| 7 | 7 |
Latest revision only has two accessible states: move and reverse. |
| 8 | 8 |
*/ |
| 9 | 9 |
|
| 10 |
/* private function */ |
|
| 11 |
void run_around_evaluate_state(void); |
|
| 10 | 12 |
|
| 11 | 13 |
void run_around_init(void) |
| 12 | 14 |
{
|
| 13 |
range_init(); |
|
| 14 |
analog_init(1); |
|
| 15 |
motors_init(); |
|
| 16 |
orb_init(); |
|
| 17 |
orb_enable(); |
|
| 15 |
//range_init();
|
|
| 16 |
//analog_init(1);
|
|
| 17 |
//motors_init();
|
|
| 18 |
//orb_init();
|
|
| 19 |
//orb_enable();
|
|
| 18 | 20 |
usb_init(); |
| 19 | 21 |
|
| 20 | 22 |
/*Start in the default state, MOVING*/ |
| ... | ... | |
| 27 | 29 |
/*Initialize distances to zero.*/ |
| 28 | 30 |
d1=0; d2=0; d3=0; d4=0; d5=0; |
| 29 | 31 |
|
| 30 |
orb_set_color(GREEN); |
|
| 32 |
//orb_set_color(GREEN);
|
|
| 31 | 33 |
|
| 32 | 34 |
} |
| 33 | 35 |
|
| ... | ... | |
| 63 | 65 |
|
| 64 | 66 |
if(crazy_count<0) crazy_count=CRAZY_MAX; |
| 65 | 67 |
|
| 66 |
evaluate_state(); |
|
| 68 |
run_around_evaluate_state();
|
|
| 67 | 69 |
return; |
| 68 | 70 |
} |
| 69 | 71 |
|
| ... | ... | |
| 71 | 73 |
if((d2!=-1)&&(d2 < 150)){
|
| 72 | 74 |
backup_count=BACKUP_MAX; |
| 73 | 75 |
avoid_state=BACKWARDS; |
| 74 |
evaluate_state(); |
|
| 76 |
run_around_evaluate_state();
|
|
| 75 | 77 |
return; |
| 76 | 78 |
} |
| 77 | 79 |
/* |
| 78 | 80 |
if(d1 < 120 || d3 < 120) {
|
| 79 | 81 |
avoid_state = BACKWARDS; |
| 80 | 82 |
backup_count = BACKUP_MAX; |
| 81 |
evaluate_state(); |
|
| 83 |
run_around_evaluate_state();
|
|
| 82 | 84 |
return; |
| 83 | 85 |
} |
| 84 | 86 |
*/ |
| ... | ... | |
| 86 | 88 |
avoid_state=BACKWARDS; |
| 87 | 89 |
if(backup_count<0) |
| 88 | 90 |
backup_count=BACKUP_MAX; |
| 89 |
evaluate_state(); |
|
| 91 |
run_around_evaluate_state();
|
|
| 90 | 92 |
return; |
| 91 | 93 |
} |
| 92 | 94 |
|
| ... | ... | |
| 112 | 114 |
usb_puti(d5); |
| 113 | 115 |
usb_puts("\n\r");*/
|
| 114 | 116 |
|
| 115 |
evaluate_state(); |
|
| 117 |
run_around_evaluate_state();
|
|
| 116 | 118 |
} |
| 117 | 119 |
|
| 118 | 120 |
|
| 119 | 121 |
//Acts on state change. |
| 120 |
void evaluate_state(){
|
|
| 122 |
void run_around_evaluate_state(){
|
|
| 121 | 123 |
switch(avoid_state){
|
| 122 |
case(MOVING): orb_set_color(GREEN); |
|
| 124 |
case(MOVING): //orb_set_color(GREEN);
|
|
| 123 | 125 |
move(STRAIT_SPEED,-pControl); |
| 124 | 126 |
break; |
| 125 | 127 |
|
| 126 |
case(BACKWARDS): orb_set_color(ORANGE); |
|
| 128 |
case(BACKWARDS): //orb_set_color(ORANGE);
|
|
| 127 | 129 |
move(-STRAIT_SPEED,0); |
| 128 | 130 |
break; |
| 129 | 131 |
|
| 130 |
case(CRAZY): orb_set_color(RED); |
|
| 132 |
case(CRAZY): //orb_set_color(RED);
|
|
| 131 | 133 |
/*TODO: Implement a crazy state.*/ |
| 132 | 134 |
move(STRAIT_SPEED,-pControl); |
| 133 | 135 |
break; |
| 134 | 136 |
|
| 135 | 137 |
default: |
| 136 | 138 |
/*Should never get here, go strait.*/ |
| 137 |
move(100,0); orb_set_color(BLUE); |
|
| 139 |
move(100,0); //orb_set_color(BLUE);
|
|
| 138 | 140 |
break; |
| 139 | 141 |
} |
| 140 | 142 |
} |
Also available in: Unified diff