Project

General

Profile

Revision 1200

Added by Abraham Levkoy almost 15 years ago

Smart run around produces more of a wall-following behavior at
this point. Rangefinder readings are clearly not in cm. I have assumed that they
give the distance in mm but 50 mm too large. I don't think this is correct,
but the robot does not crash, so I left. The states do what they are
supposed and, for the most part, get triggered when they are supposed. The
forward state does not work (it just stops) if the speed is HALF_SPD rather
than FULL_SPD.

View differences:

trunk/code/projects/mapping/new_run_around/smart_run_around_fsm.c
39 39 
    temp = range_read_distance(IR5);
40 40 
    d5 = (temp == -1) ? d5 : temp;
41 41 

  		




  42
  
  
    
    // TODO: double-check rangefinder numbers and positions


  		




  43
  42
  
    
    // update state based on rangefinder input


  		




  44
  43
  
    
    if (d2 < IR2_DANGER)    // avoid frontal collision by turning in place


  		




  45
  44
  
    
	state = SPIN;


  		




  ......




  48
  47
  
    
     */


  		




  49
  48
  
    
    else if (d1 < IR13_DANGER && d3 < IR13_DANGER)


  		




  50
  49
  
    
	state = FORWARD;


  		




  51
  
  
    
    else if (d1 < IR13_DANGER)	// avoid right-side collision by turning left


  		




  
  50
  
    
    else if (d1 < IR13_DANGER)	// avoid left-side collision by turning right


  		




  
  51
  
    
	state = RIGHT;


  		




  
  52
  
    
    else if (d3 < IR13_DANGER)	// avoid right-side collision by turning left


  		




  52
  53
  
    
	state = LEFT;


  		




  53
  
  
    
    else if (d3 < IR13_DANGER)	// avoid left-side collision by turn right


  		




  54
  
  
    
	state = RIGHT;


  		




  55
  54
  
    
    else if (d2 < IR2_INTEREST)	// should turn to avoid obstacle up ahead


  		




  56
  55
  
    
    {


  		




  57
  56
  
    
	if (d3 >= d1)	// more room on right


  		




  ......




  75
  74
  
    
    usb_puti(d5);


  		




  76
  75
  
    
    usb_puts("\n\r");


  		




  77
  76
  
    

  		




  
  77
  
    
    if (button2_read())	// pushing buttons resets robot


  		




  
  78
  
    
    	reset();


  		




  
  79
  
    

  		




  78
  80
  
    
    evaluate_state();	// take action on updated state


  		




  79
  81
  
    
}


  		




  80
  82
  
    

  		




  81
  83
  
    
// behave according to current state


  		




  82
  
  
    
// TODO: double-check orb numbers


  		




  83
  84
  
    
// TODO: adjust speeds after testing


  		




  84
  85
  
    
void evaluate_state(void)


  		




  85
  86
  
    
{


  		




  ......




  87
  88
  
    
    {


  		




  88
  89
  
    
	case FORWARD:


  		




  89
  90
  
    
	    orb_set_color(GREEN);


  		




  90
  
  
    
	    move(HALF_SPD, 0);	// drive straight forward


  		




  
  91
  
    
	    move(FULL_SPD, 0);	// drive straight forward


  		




  91
  92
  
    
	    break;


  		




  92
  93
  
    
	case LEFT:


  		




  93
  
  
    
	    orbs_set_color(YELLOW, GREEN);  // green on left side


  		




  
  94
  
    
	    orbs_set_color(GREEN, YELLOW);  // green on left side


  		




  94
  95
  
    
	    move(HALF_SPD, NRML_TURN);	// drive forward and to the left


  		




  95
  96
  
    
	    break;


  		




  96
  97
  
    
	case RIGHT:


  		




  97
  
  
    
	    orbs_set_color(GREEN, YELLOW);  // green on right side


  		




  
  98
  
    
	    orbs_set_color(YELLOW, GREEN);  // green on right side


  		




  98
  99
  
    
	    move(HALF_SPD, -NRML_TURN);	// drive forward and to the right


  		




  99
  100
  
    
	    break;


  		




  100
  101
  
    
	case SPIN:


  		












  

    
      trunk/code/projects/mapping/new_run_around/smart_run_around_fsm.h
    
  





  12
  12
  
    
#define RIGHT 2	// drive forward and to the right


  		




  13
  13
  
    
#define SPIN 3	// turn without traveling (last resort if otherwise stuck)


  		




  14
  14
  
    

  		




  15
  
  
    
/* conditions on rangefinders (in cm) informing state changes


  		




  
  15
  
    
/* conditions on rangefinders (in mm) informing state changes


  		




  
  16
  
    
 * distances are 50 greater than in real life, cannot be smaller than 100


  		




  16
  17
  
    
 * naming format: SENSOR_URGENCY


  		




  17
  18
  
    
 * changing these constants affects robot operation


  		




  18
  19
  
    
 */


  		




  19
  
  
    
#define IR2_DANGER 5	// dangerously close to front


  		




  20
  
  
    
#define IR13_DANGER 10  // dangerously close to sides (looking ahead)


  		




  21
  
  
    
#define IR2_INTEREST 15	// notably close to front


  		




  22
  
  
    
#define IR45_DANGER 5	// dangerously close to sides (looking sideways)


  		




  
  20
  
    
#define IR2_DANGER 150	// 10 cm: dangerously close to front


  		




  
  21
  
    
#define IR13_DANGER 200 // 15 cm: dangerously close to sides (looking ahead)


  		




  
  22
  
    
#define IR2_INTEREST 300    // 25 cm: notably close to front


  		




  
  23
  
    
#define IR45_DANGER 150	// dangerously close to side (looking sideways)


  		




  23
  24
  
    

  		




  24
  25
  
    
uint8_t state;	// current state of FSM: FORWARD, LEFT, RIGHT, SPIN


  		




  25
  26
  
    
int16_t d1, d2, d3, d4, d5; // values returned by rangefinders


  		












Also available in:  Unified diff