root / scout / libscout / src / behaviors / Odometry.cpp @ d140fd71
History | View | Annotate | Download (1.63 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 | 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 | 31be19a6 | Priya | 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 | f79fbce2 | Priya | |
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 | 31be19a6 | Priya | 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 | f79fbce2 | Priya | return;
|
50 | } |
||
51 | |||
52 | void Odometry::run()
|
||
53 | { |
||
54 | while(ok())
|
||
55 | { |
||
56 | get_position(); |
||
57 | 31be19a6 | Priya | //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 | f79fbce2 | Priya | } |
60 | } |