scoutos / scout / libscout / src / behaviors / Odometry.cpp @ 30a3768e
History | View | Annotate | Download (2.31 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 | ROS_INFO("fl %d fr %d bl %d br %d", scout_enc.fl_ticks, scout_enc.fr_ticks, scout_enc.bl_ticks, scout_enc.br_ticks);
|
27 | ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_ticks, motor_fr_ticks, motor_bl_ticks, motor_br_ticks);
|
||
28 | ROS_INFO("fl %d fr %d bl %d br %d", motor_fl_dist, motor_fr_dist, motor_bl_dist, motor_br_dist);
|
||
29 | f79fbce2 | Priya | |
30 | 3db79f25 | Priya | // Get Left and Right distance
|
31 | left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + |
||
32 | ((float)(motor_bl_dist))/2.0); |
||
33 | right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + |
||
34 | ((float)(motor_br_dist))/2.0); |
||
35 | f79fbce2 | Priya | |
36 | 3db79f25 | Priya | total_dist = (left_dist)/2.0 + (right_dist)/2.0; |
37 | theta = scout_pos->theta + atan2(left_dist - right_dist, WHEEL_BASE); |
||
38 | |||
39 | ROS_INFO("l: %f r: %f theta: %f", left_dist, right_dist, theta);
|
||
40 | |||
41 | scout_pos->x += total_dist*sin(theta); |
||
42 | scout_pos->y += total_dist*cos(theta); |
||
43 | scout_pos->theta = fmod(theta, 2*M_PI);
|
||
44 | 31be19a6 | Priya | |
45 | //Save state for next time in.
|
||
46 | motor_fl_ticks = scout_enc.fl_ticks; |
||
47 | motor_fr_ticks = scout_enc.fr_ticks; |
||
48 | motor_bl_ticks = scout_enc.bl_ticks; |
||
49 | motor_br_ticks = scout_enc.br_ticks; |
||
50 | 3db79f25 | Priya | |
51 | |||
52 | f79fbce2 | Priya | return;
|
53 | } |
||
54 | |||
55 | void Odometry::run()
|
||
56 | { |
||
57 | 30a3768e | Priya | scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); |
58 | |||
59 | f79fbce2 | Priya | while(ok())
|
60 | { |
||
61 | get_position(); |
||
62 | 30a3768e | Priya | |
63 | position.name = name; |
||
64 | position.x = scout_pos->x; |
||
65 | position.y = scout_pos->y; |
||
66 | position.theta = scout_pos->theta; |
||
67 | scout_position.publish(position); |
||
68 | |||
69 | 31be19a6 | Priya | ROS_INFO("Scout is at x: %f y: %f theta: %f\n", scout_pos->x, scout_pos->y, scout_pos->theta);
|
70 | f79fbce2 | Priya | } |
71 | } |