root / scout / libscout / src / behaviors / Odometry.cpp @ d7e1b0d8
History | View | Annotate | Download (2.58 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 | 751d4f4f | Alex | Odometry::Odometry(string scoutname, Sensors* sensors)
|
8 | : Behavior(scoutname, "odometry", sensors)
|
||
9 | { |
||
10 | d7e1b0d8 | Alex | name = scoutname; |
11 | scout_pos = new pos;
|
||
12 | motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
||
13 | 751d4f4f | Alex | } |
14 | |||
15 | /** Set up the odometry node and prepare communcations over ROS */
|
||
16 | d7e1b0d8 | Alex | Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors) |
17 | : Behavior(scoutname, behavior_name, sensors) |
||
18 | f79fbce2 | Priya | { |
19 | d7e1b0d8 | Alex | name = scoutname; |
20 | scout_pos = new pos;
|
||
21 | motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
|
||
22 | } |
||
23 | |||
24 | void Odometry::wait(float duration) |
||
25 | { |
||
26 | ros::Rate r(WAIT_HZ); |
||
27 | int ticks = int(duration * WAIT_HZ); |
||
28 | for (int i = 0; i < ticks; i++) |
||
29 | { |
||
30 | get_position(); |
||
31 | spinOnce(); |
||
32 | r.sleep(); |
||
33 | } |
||
34 | f79fbce2 | Priya | } |
35 | |||
36 | /** Query encoders and estimate position based on encoder reading */
|
||
37 | void Odometry::get_position()
|
||
38 | { |
||
39 | d7e1b0d8 | Alex | float left_dist, right_dist, total_dist, theta;
|
40 | |||
41 | encoder_readings scout_enc = encoders->query(); |
||
42 | motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks; |
||
43 | motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks; |
||
44 | motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks; |
||
45 | motor_br_dist = scout_enc.br_ticks - motor_br_ticks; |
||
46 | |||
47 | // Get Left and Right distance
|
||
48 | left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 + |
||
49 | ((float)(motor_bl_dist))/2.0); |
||
50 | right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + |
||
51 | ((float)(motor_br_dist))/2.0); |
||
52 | |||
53 | total_dist = (left_dist)/2.0 + (right_dist)/2.0; |
||
54 | theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE); |
||
55 | |||
56 | //Use negative theta because we measure theta from 0 on the
|
||
57 | //right to 180 on the left counter-clock-wise, but this is
|
||
58 | //negative theta in the coordinate frame.
|
||
59 | //Also, subtract the delta from y because positive y is down.
|
||
60 | scout_pos->x += total_dist*cos(-theta); |
||
61 | scout_pos->y -= total_dist*sin(-theta); |
||
62 | scout_pos->theta = fmod(theta, (float)(2*M_PI)); |
||
63 | |||
64 | //Save state for next time in.
|
||
65 | motor_fl_ticks = scout_enc.fl_ticks; |
||
66 | motor_fr_ticks = scout_enc.fr_ticks; |
||
67 | motor_bl_ticks = scout_enc.bl_ticks; |
||
68 | motor_br_ticks = scout_enc.br_ticks; |
||
69 | |||
70 | |||
71 | return;
|
||
72 | f79fbce2 | Priya | } |
73 | |||
74 | void Odometry::run()
|
||
75 | { |
||
76 | d7e1b0d8 | Alex | scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); |
77 | |||
78 | while(ok())
|
||
79 | { |
||
80 | get_position(); |
||
81 | |||
82 | position.name = name; |
||
83 | position.x = scout_pos->x; |
||
84 | position.y = scout_pos->y; |
||
85 | position.theta = scout_pos->theta; |
||
86 | scout_position.publish(position); |
||
87 | } |
||
88 | f79fbce2 | Priya | } |