Project

General

Profile

Statistics
| Revision:

root / branches / simulator / projects / template / smart_run_around_fsm.c @ 1129

History | View | Annotate | Download (2.98 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
}
26

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

    
89
  if(pControl > 255)
90
    pControl = 255;
91
  if(pControl < -255)
92
    pControl = -255;
93
  
94
  if(pControl>PCONTROL_CRAZY_LIMIT || pControl<-PCONTROL_CRAZY_LIMIT) crazy_count--;
95
  /*i.e. if you really want to turn for an extended period of time...you're probably stuck.*/
96

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

    
116

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

    
140