Revision 695
minor changes to autonomous recharging on the dragonfly. still experiencing problems with run around.
| smart_run_around_fsm.c | ||
|---|---|---|
| 21 | 21 |
avoid_state=MOVING; |
| 22 | 22 |
/*Set timers to their maximum values.*/ |
| 23 | 23 |
crazy_count=CRAZY_MAX; |
| 24 |
backup_count=0;
|
|
| 24 |
backup_count=BACKUP_MAX;
|
|
| 25 | 25 |
pControl=0; |
| 26 | 26 |
|
| 27 | 27 |
/*Initialize distances to zero.*/ |
| ... | ... | |
| 33 | 33 |
|
| 34 | 34 |
/*The main function, call this to update states as frequently as possible.*/ |
| 35 | 35 |
void run_around_FSM(void) {
|
| 36 |
/*Default to moving.*/
|
|
| 36 |
/*Default to moving.*/ |
|
| 37 | 37 |
avoid_state=MOVING; |
| 38 | 38 |
|
| 39 | 39 |
/*The following lines ensure that undefined (-1) values |
| ... | ... | |
| 41 | 41 |
int temp; |
| 42 | 42 |
|
| 43 | 43 |
temp=range_read_distance(IR1); |
| 44 |
d1=(temp == -1) ? d1 : temp; |
|
| 44 |
//d1=(temp == -1) ? d1 : temp; |
|
| 45 |
d1 = temp; |
|
| 45 | 46 |
|
| 46 | 47 |
temp=range_read_distance(IR2); |
| 47 |
d2=(temp == -1) ? d2 : temp; |
|
| 48 |
//d2=(temp == -1) ? d2 : temp; |
|
| 49 |
d2 = temp; |
|
| 48 | 50 |
|
| 49 | 51 |
temp=range_read_distance(IR3); |
| 50 |
d3=(temp == -1) ? d3 : temp; |
|
| 52 |
//d3=(temp == -1) ? d3 : temp; |
|
| 53 |
d3 = temp; |
|
| 51 | 54 |
|
| 52 | 55 |
temp=range_read_distance(IR4); |
| 53 |
d4=(temp == -1) ? d4 : temp; |
|
| 56 |
//d4=(temp == -1) ? d4 : temp; |
|
| 57 |
d4 = temp; |
|
| 54 | 58 |
|
| 55 | 59 |
temp=range_read_distance(IR5); |
| 56 |
d5=(temp == -1) ? d5 : temp; |
|
| 60 |
//d5=(temp == -1) ? d5 : temp; |
|
| 61 |
d5 = temp; |
|
| 57 | 62 |
|
| 58 | 63 |
//If the crazy count is in it's >>3 range, it acts crazy. |
| 59 | 64 |
if(crazy_count<=(CRAZY_MAX>>3)) |
| ... | ... | |
| 68 | 73 |
} |
| 69 | 74 |
|
| 70 | 75 |
//Checks the forward distance to see if it should back up, if so...state backwards. |
| 71 |
if((d2!=-1)&&(d2 < 150)){
|
|
| 76 |
if((d2!=-1)&&(d2 < 200)){
|
|
| 72 | 77 |
backup_count=BACKUP_MAX; |
| 73 | 78 |
avoid_state=BACKWARDS; |
| 74 | 79 |
evaluate_state(); |
| 75 | 80 |
return; |
| 76 | 81 |
} |
| 77 | 82 |
if(backup_count<BACKUP_MAX){
|
| 78 |
avoid_state=BACKWARDS;
|
|
| 83 |
avoid_state=BACKWARDS; |
|
| 79 | 84 |
if(backup_count<0) |
| 80 | 85 |
backup_count=BACKUP_MAX; |
| 81 | 86 |
evaluate_state(); |
| ... | ... | |
| 116 | 121 |
break; |
| 117 | 122 |
|
| 118 | 123 |
case(BACKWARDS): orb_set_color(ORANGE); |
| 119 |
move(-STRAIT_SPEED,0);
|
|
| 124 |
move(BACK_SPEED,0);
|
|
| 120 | 125 |
break; |
| 121 | 126 |
|
| 122 | 127 |
case(CRAZY): orb_set_color(RED); |
Also available in: Unified diff