Revision ddfeb111 scout/libscout/src/behaviors/Odometry.cpp

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