Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / Odometry.cpp @ 751d4f4f

History | View | Annotate | Download (2.31 KB)

1

    
2
#include "Odometry.h"
3

    
4
using namespace std;
5

    
6
/** Set up the odometry node and prepare communcations over ROS */
7
Odometry::Odometry(string scoutname, Sensors* sensors)
8
    : Behavior(scoutname, "odometry", sensors)
9
{
10
  name = scoutname;
11
  scout_pos = new pos;
12
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
13
}
14

    
15
/** Set up the odometry node and prepare communcations over ROS */
16
Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
17
    : Behavior(scoutname, behavior_name, sensors)
18
{
19
  name = scoutname;
20
  scout_pos = new pos;
21
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
22
}
23

    
24
/** Query encoders and estimate position based on encoder reading */
25
void Odometry::get_position()
26
{
27
  float left_dist, right_dist, total_dist, theta;
28

    
29
  encoder_readings scout_enc = encoders->query();
30
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
31
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
32
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
33
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
34

    
35
  // Get Left and Right distance
36
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
37
                             ((float)(motor_bl_dist))/2.0);
38
  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
39
                              ((float)(motor_br_dist))/2.0);
40

    
41
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
42
  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
43

    
44
  //Use negative theta because we measure theta from 0 on the
45
  //right to 180 on the left counter-clock-wise, but this is
46
  //negative theta in the coordinate frame.
47
  //Also, subtract the delta from y because positive y is down.
48
  scout_pos->x += total_dist*cos(-theta);
49
  scout_pos->y -= total_dist*sin(-theta);
50
  scout_pos->theta = fmod(theta, (float)(2*M_PI));
51

    
52
  //Save state for next time in.
53
  motor_fl_ticks = scout_enc.fl_ticks;
54
  motor_fr_ticks = scout_enc.fr_ticks;
55
  motor_bl_ticks = scout_enc.bl_ticks;
56
  motor_br_ticks = scout_enc.br_ticks;
57

    
58

    
59
 return;
60
}
61

    
62
void Odometry::run()
63
{
64
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); 
65

    
66
  while(ok())
67
  {
68
    get_position();
69

    
70
    position.name = name;
71
    position.x = scout_pos->x;
72
    position.y = scout_pos->y;
73
    position.theta = scout_pos->theta;
74
    scout_position.publish(position);
75
  }
76
}