Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / Odometry.cpp @ d140fd71

History | View | Annotate | Download (1.63 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):Behavior(scoutname, 
8
                                                "odometry", sensors)
9
{
10
  scout_pos = new pos;
11
}
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
//  float left_vel, right_vel;
18

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

    
32
  left_dist = DIST_PER_TICK*(motor_fl_dist + motor_bl_dist)/2;
33
  right_dist = DIST_PER_TICK*(motor_fr_dist + motor_br_dist)/2;
34

    
35
  total_dist = (left_dist+right_dist)/2;
36
  theta = (right_dist - left_dist)/WHEEL_BASE;
37

    
38
  scout_pos->x = total_dist*sin(theta);
39
  scout_pos->y = total_dist*cos(theta);
40
  scout_pos->theta = theta;
41

    
42
/**
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
**/
49
 return;
50
}
51

    
52
void Odometry::run()
53
{
54
  while(ok())
55
  {
56
    get_position();
57
    //TODO publish position
58
    ROS_INFO("Scout is at x: %f  y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
59
  }
60
}