Project

General

Profile

Revision ddfeb111

IDddfeb11183e90127649a5f69425bedc926bda71c

Added by Priya about 4 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