Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Odometry.cpp @ 3db79f25

History | View | Annotate | Download (1.99 KB)

1 f79fbce2 Priya
2
#include "Odometry.h"
3
4
using namespace std;
5
6
/** Set up the odometry node and prepare communcations over ROS */
7 31be19a6 Priya
Odometry::Odometry(string scoutname):Behavior(scoutname, "odometry")
8 f79fbce2 Priya
{
9
  scout_pos = new pos;
10 3db79f25 Priya
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
11 f79fbce2 Priya
}
12
13
/** Query encoders and estimate position based on encoder reading */
14
void Odometry::get_position()
15
{
16
  float left_dist, right_dist, total_dist, theta;
17
18
  encoder_readings scout_enc = encoders->query();
19 31be19a6 Priya
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
20
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
21
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
22
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
23 f79fbce2 Priya
24 3db79f25 Priya
  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);
27 f79fbce2 Priya
28 3db79f25 Priya
  // 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);
33 f79fbce2 Priya
34 3db79f25 Priya
  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);
42 31be19a6 Priya
43
  //Save state for next time in.
44
  motor_fl_ticks = scout_enc.fl_ticks;
45
  motor_fr_ticks = scout_enc.fr_ticks;
46
  motor_bl_ticks = scout_enc.bl_ticks;
47
  motor_br_ticks = scout_enc.br_ticks;
48 3db79f25 Priya
49
50 f79fbce2 Priya
 return;
51
}
52
53
void Odometry::run()
54
{
55
  while(ok())
56
  {
57
    get_position();
58 31be19a6 Priya
    //TODO publish position
59
    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
60 f79fbce2 Priya
  }
61
}