Revision 3db79f25 scout/libscout/src/behaviors/Odometry.cpp

View differences:

scout/libscout/src/behaviors/Odometry.cpp
7 7
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry")
8 8
{
9 9
  scout_pos = new pos;
10
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
10 11
}
11 12

  
12 13
/** Query encoders and estimate position based on encoder reading */
13 14
void Odometry::get_position()
14 15
{
15 16
  float left_dist, right_dist, total_dist, theta;
16
//  float left_vel, right_vel;
17 17

  
18
  /* TODO: Get current encoder ticks */
19 18
  encoder_readings scout_enc = encoders->query();
20
  motor_fl_dist = scout_enc.fl_ticks;
21
  motor_fr_dist = scout_enc.fr_ticks;
22
  motor_bl_dist = scout_enc.bl_ticks;
23
  motor_br_dist = scout_enc.br_ticks;
24
/**
25 19
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
26 20
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
27 21
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
28 22
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
29
**/
30 23

  
31
  left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2;
32
  right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2;
24
  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);
25
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
26
  ROS_INFO("fl %d  fr  %d  bl  %d  br  %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
33 27

  
34
  total_dist = (left_dist+right_dist)/2;
35
  theta = (right_dist - left_dist)/WHEEL_BASE;
28
  // Get Left and Right distance
29
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
30
                             ((float)(motor_bl_dist))/2.0);
31
  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
32
                              ((float)(motor_br_dist))/2.0);
36 33

  
37
  scout_pos->x = total_dist*sin(theta);
38
  scout_pos->y = total_dist*cos(theta);
39
  scout_pos->theta = theta;
34
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
35
  theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE);
36

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

  
39
  scout_pos->x += total_dist*sin(theta);
40
  scout_pos->y += total_dist*cos(theta);
41
  scout_pos->theta = fmod(theta, 2*M_PI);
40 42

  
41
/**
42 43
  //Save state for next time in.
43 44
  motor_fl_ticks = scout_enc.fl_ticks;
44 45
  motor_fr_ticks = scout_enc.fr_ticks;
45 46
  motor_bl_ticks = scout_enc.bl_ticks;
46 47
  motor_br_ticks = scout_enc.br_ticks;
47
**/
48

  
49

  
48 50
 return;
49 51
}
50 52

  

Also available in: Unified diff