Statistics
| Branch: | Revision:

root / scout / libscout / src / behaviors / Odometry.cpp @ 339f64d2

History | View | Annotate | Download (2.05 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 d140fd71 Yuyang
Odometry::Odometry(string scoutname, Sensors* sensors):Behavior(scoutname, 
8
                                                "odometry", sensors)
9 f79fbce2 Priya
{
10 30a3768e Priya
  name = scoutname;
11 f79fbce2 Priya
  scout_pos = new pos;
12 3db79f25 Priya
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
13 f79fbce2 Priya
}
14
15
/** Query encoders and estimate position based on encoder reading */
16
void Odometry::get_position()
17
{
18
  float left_dist, right_dist, total_dist, theta;
19
20
  encoder_readings scout_enc = encoders->query();
21 31be19a6 Priya
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
22
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
23
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
24
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
25 f79fbce2 Priya
26 3db79f25 Priya
  // Get Left and Right distance
27
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
28
                             ((float)(motor_bl_dist))/2.0);
29
  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
30
                              ((float)(motor_br_dist))/2.0);
31 f79fbce2 Priya
32 3db79f25 Priya
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
33 ddfeb111 Priya
  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);
41 339f64d2 Alex
  scout_pos->theta = fmod(theta, (float)(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 30a3768e Priya
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); 
56
57 f79fbce2 Priya
  while(ok())
58
  {
59
    get_position();
60 30a3768e Priya
61
    position.name = name;
62
    position.x = scout_pos->x;
63
    position.y = scout_pos->y;
64
    position.theta = scout_pos->theta;
65
    scout_position.publish(position);
66 f79fbce2 Priya
  }
67
}