root / 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 |
} |