Project

General

Profile

Revision 211

Modified smart_run_around_fsm to fix the direction and make it smoother

View differences:

trunk/code/behaviors/smart_run_around_fsm/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:
trunk/code/behaviors/smart_run_around_fsm/smart_run_around_fsm.h
13 13
#define LEFT 37             //Left
14 14
#define RIGHT 39            //Right
15 15

  
16
#define BACKUP_MAX 15
16
#define BACKUP_MAX 20
17 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.
18
#define STRAIT_SPEED 185    //The speed when going strait or backing up.
19 19
#define TURN_CONSTANT 2
20 20
#define PCONTROL_CRAZY_LIMIT 80
21 21

  

Also available in: Unified diff