Project

General

Profile

Revision ddfeb111

IDddfeb11183e90127649a5f69425bedc926bda71c

Added by Priya over 11 years ago

Fixing bugs with ghost scout (these were problems in odometry related
to constants in the simulator and coordinate frame and theta representation
in the simulator). Also made ghost scout translucent in order to differentiate
it.

View differences:

scout/libscout/src/behaviors/Odometry.cpp
23 23 
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
24 24 
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
25 25 

  		




  26
  
  
    
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);


  		




  27
  
  
    
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);


  		




  28
  
  
    
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);


  		




  29
  
  
    

  		




  30
  26
  
    
  // Get Left and Right distance


  		




  31
  27
  
    
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +


  		




  32
  28
  
    
                             ((float)(motor_bl_dist))/2.0);


  		




  ......




  34
  30
  
    
                              ((float)(motor_br_dist))/2.0);


  		




  35
  31
  
    

  		




  36
  32
  
    
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;


  		




  37
  
  
    
  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);


  		




  38
  
  
    

  		




  39
  
  
    
  ROS_INFO("l: %f   r: %f   theta: %f", left_dist, right_dist, theta);


  		




  40
  
  
    

  		




  41
  
  
    
  scout_pos->x += total_dist*sin(theta);


  		




  42
  
  
    
  scout_pos->y += total_dist*cos(theta);


  		




  
  33
  
    
  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);


  		




  
  34
  
    

  		




  
  35
  
    
  //Use negative theta because we measure theta from 0 on the


  		




  
  36
  
    
  //right to 180 on the left counter-clock-wise, but this is


  		




  
  37
  
    
  //negative theta in the coordinate frame.


  		




  
  38
  
    
  //Also, subtract the delta from y because positive y is down.


  		




  
  39
  
    
  scout_pos->x += total_dist*cos(-theta);


  		




  
  40
  
    
  scout_pos->y -= total_dist*sin(-theta);


  		




  43
  41
  
    
  scout_pos->theta = fmod(theta, 2*M_PI);


  		




  44
  42
  
    

  		




  45
  43
  
    
  //Save state for next time in.


  		




  ......




  65
  63
  
    
    position.y = scout_pos->y;


  		




  66
  64
  
    
    position.theta = scout_pos->theta;


  		




  67
  65
  
    
    scout_position.publish(position);


  		




  68
  
  
    

  		




  69
  
  
    
    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);


  		




  70
  66
  
    
  }


  		




  71
  67
  
    
}


  		












Also available in:  Unified diff