Revision 211
Modified smart_run_around_fsm to fix the direction and make it smoother
| smart_run_around_fsm.c | ||
|---|---|---|
| 68 | 68 |
} |
| 69 | 69 |
|
| 70 | 70 |
//Checks the forward distance to see if it should back up, if so...state backwards. |
| 71 |
if((d2!=-1)&&(d2 >150)){
|
|
| 71 |
if((d2!=-1)&&(d2 < 150)){
|
|
| 72 | 72 |
backup_count=BACKUP_MAX; |
| 73 | 73 |
avoid_state=BACKWARDS; |
| 74 | 74 |
evaluate_state(); |
| 75 | 75 |
return; |
| 76 | 76 |
} |
| 77 |
|
|
| 77 |
/* |
|
| 78 |
if(d1 < 120 || d3 < 120) {
|
|
| 79 |
avoid_state = BACKWARDS; |
|
| 80 |
backup_count = BACKUP_MAX; |
|
| 81 |
evaluate_state(); |
|
| 82 |
return; |
|
| 83 |
} |
|
| 84 |
*/ |
|
| 78 | 85 |
if(backup_count<BACKUP_MAX){
|
| 79 | 86 |
avoid_state=BACKWARDS; |
| 80 | 87 |
if(backup_count<0) |
| ... | ... | |
| 113 | 120 |
void evaluate_state(){
|
| 114 | 121 |
switch(avoid_state){
|
| 115 | 122 |
case(MOVING): orb_set_color(GREEN); |
| 116 |
move(STRAIT_SPEED,pControl); |
|
| 123 |
move(STRAIT_SPEED,-pControl);
|
|
| 117 | 124 |
break; |
| 118 | 125 |
|
| 119 | 126 |
case(BACKWARDS): orb_set_color(ORANGE); |
| ... | ... | |
| 122 | 129 |
|
| 123 | 130 |
case(CRAZY): orb_set_color(RED); |
| 124 | 131 |
/*TODO: Implement a crazy state.*/ |
| 125 |
move(STRAIT_SPEED,pControl); |
|
| 132 |
move(STRAIT_SPEED,-pControl);
|
|
| 126 | 133 |
break; |
| 127 | 134 |
|
| 128 | 135 |
default: |
Also available in: Unified diff