Statistics
| Revision:

root / demos / smart_run_around_fsm / src / smart_run_around_fsm.c @ 1784

History | View | Annotate | Download (3.01 KB)

1
#include "dragonfly_lib.h"
2
#include "smart_run_around_fsm.h"
3

    
4
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck.
5
Could be better at not getting stuck.
6

7
Latest revision only has two accessible states: move and reverse.
8
*/
9

    
10

    
11
void run_around_init(void)
12
{
13
  range_init();
14
 
15
  /*Start in the default state, MOVING*/ 
16
  avoid_state=MOVING;
17
  /*Set timers to their maximum values.*/
18
  crazy_count=CRAZY_MAX;
19
  backup_count=BACKUP_MAX; 
20
  pControl=0;
21
  
22
  /*Initialize distances to zero.*/ 
23
  d1=0; d2=0; d3=0; d4=0; d5=0;
24
  
25
  orb_set_color(GREEN);
26

    
27
}
28

    
29
/*The main function, call this to update states as frequently as possible.*/
30
void run_around_FSM(void) {
31
  /*Default to moving.*/ 
32
  avoid_state=MOVING;
33
  
34
  /*The following lines ensure that undefined (-1) values
35
  will not update the distances.*/ 
36
  int temp;
37
  
38
  temp=range_read_distance(IR1);
39
  d1=(temp == -1) ? d1 : temp;
40
  
41
  temp=range_read_distance(IR2);
42
  d2=(temp == -1) ? d2 : temp;
43
  
44
  temp=range_read_distance(IR3);
45
  d3=(temp == -1) ? d3 : temp;
46
  
47
  temp=range_read_distance(IR4);
48
  d4=(temp == -1) ? d4 : temp;
49
  
50
  temp=range_read_distance(IR5);
51
  d5=(temp == -1) ? d5 : temp;
52
  
53
  /*If the crazy count is in it's >>3 range, it acts crazy.*/
54
  if(crazy_count<=(CRAZY_MAX>>3))
55
  {
56
    avoid_state=CRAZY;
57
    crazy_count--;
58
    
59
    if(crazy_count<0) crazy_count=CRAZY_MAX;
60
    
61
    evaluate_state();
62
    return;
63
  }
64
  
65
  //Checks the forward distance to see if it should back up, if so...state backwards.
66
  if((d2!=-1)&&(d2 < 150)){
67
      backup_count=BACKUP_MAX;
68
      avoid_state=BACKWARDS;
69
      evaluate_state();
70
      return;
71
  }
72
  /*
73
  if(d1 < 120 || d3 < 120) {
74
                avoid_state = BACKWARDS;
75
                backup_count = BACKUP_MAX;
76
                evaluate_state();
77
                return;
78
  }
79
  */
80
  if(backup_count<BACKUP_MAX){
81
    avoid_state=BACKWARDS; 
82
    if(backup_count<0)
83
      backup_count=BACKUP_MAX;
84
    evaluate_state();
85
    return;
86
  }
87
  
88
  /*Should evaluate an expression from -255 to 255 to pass to move.*/
89
  pControl= ((d3-d1) + (d4-d5)) >> TURN_CONSTANT;
90
  
91
  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;
92
  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/
93

    
94
  /*Debug stuff:*/
95
  /*usb_puts("pControl evaluating: ");
96
  usb_puti(pControl);
97
  usb_puts("\n\r");
98
  usb_puts("IR1: ");
99
  usb_puti(d1);
100
  usb_puts(" IR2: ");
101
  usb_puti(d2);
102
  usb_puts(" IR3: ");
103
  usb_puti(d3);
104
  usb_puts(" IR4: ");
105
  usb_puti(d4);
106
  usb_puts(" IR5: ");
107
  usb_puti(d5);
108
  usb_puts("\n\r");*/
109
  
110
  evaluate_state();
111
}
112

    
113

    
114
//Acts on state change.
115
void evaluate_state(){
116
    switch(avoid_state){
117
    case(MOVING): orb_set_color(GREEN);
118
      move(STRAIT_SPEED,-pControl);
119
      break;
120
    
121
    case(BACKWARDS): orb_set_color(MAGENTA);
122
      move(-STRAIT_SPEED-50,0);
123
      break;
124
      
125
    case(CRAZY): orb_set_color(RED);
126
      /*TODO: Implement a crazy state.*/
127
      move(STRAIT_SPEED,-pControl);
128
      break;
129
      
130
    default:
131
      /*Should never get here, go strait.*/
132
      move(100,0); orb_set_color(BLUE);
133
      break;
134
  }
135
}
136

    
137