Project

General

Profile

Revision 94

Added by James Kong about 15 years ago

Added smart_run_around_fsm to behaviors in trunk

View differences:

trunk/code/behaviors/smart_run_around_fsm/smart_run_around_fsm.c
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
  analog_init();
15
  motors_init();
16
  orb_init();
17
  orb_enable();
18
  usb_init();
19
 
20
  /*Start in the default state, MOVING*/ 
21
  avoid_state=MOVING;
22
  /*Set timers to their maximum values.*/
23
  crazy_count=CRAZY_MAX;
24
  backup_count=0; 
25
  pControl=0;
26
  
27
  /*Initialize distances to zero.*/ 
28
  d1=0; d2=0; d3=0; d4=0; d5=0;
29
  
30
  orb_set_color(GREEN);
31

  
32
}
33

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

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

  
111

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

  
135

  
trunk/code/behaviors/smart_run_around_fsm/smart_run_around_fsm.h
1
//Obstacle Avoid Numbers
2

  
3

  
4
#ifndef _RUN_AROUND_FSM_H_
5
#define _RUN_AROUND_FSM_H_
6

  
7
//The States: 
8
#define MOVING 12           //Move strait.
9
#define BACKWARDS 15        //Move backwards. (Front close to wall.)
10
#define STOP 16             //Stop.  The default state, (Something broke).
11
#define CRAZY 40            //Erratic behavior that occurs more often when the robot is frequently trying to turn. (i.e. may be stuck.)
12

  
13
#define LEFT 37             //Left
14
#define RIGHT 39            //Right
15

  
16
#define BACKUP_MAX 15
17
#define CRAZY_MAX 200       //The number of counts between "crazy moments"
18
#define STRAIT_SPEED 200    //The speed when going strait or backing up.
19
#define TURN_CONSTANT 2
20
#define PCONTROL_CRAZY_LIMIT 80
21

  
22
int avoid_state;    /*State machine variable.*/
23
int crazy_count;    /*Counter for a 'get unstuck' behavior.*/
24

  
25
int backup_count;	/*Counter for backup duration.*/
26
int pControl;		/*Proportional control variable, determines turn direction.*/
27
int d1,d2,d3,d4,d5;	/*The five distances taken in by IR.*/
28

  
29
void run_around_init(void);
30
void run_around_FSM(void);
31
void evaluate_state(void);
32

  
33
#endif

Also available in: Unified diff