scoutos / scout / libscout / src / test_behaviors / Odometry_new.cpp @ bb64f5e5
History | View | Annotate | Download (3.53 KB)
1 |
|
---|---|
2 |
#include "Odometry_new.h" |
3 |
|
4 |
using namespace std; |
5 |
|
6 |
/** Set up the odometry node and prepare communications over ROS */
|
7 |
Odometry_new::Odometry_new(string scoutname, Sensors* sensors):
|
8 |
Behavior(scoutname, "odometry", sensors)
|
9 |
{ |
10 |
name = scoutname; |
11 |
scout_pos = new pos;
|
12 |
|
13 |
encoder_readings scout_enc = encoders->query(); |
14 |
motor_fl_ticks_last = motor_bl_ticks_last = scout_enc.fl_ticks; |
15 |
motor_fr_ticks_last = motor_br_ticks_last = scout_enc.fr_ticks; |
16 |
} |
17 |
|
18 |
|
19 |
/** Query encoders and integrate using RK4 to estimate position */
|
20 |
void Odometry_new::get_position(double loop_time) |
21 |
{ |
22 |
float avg_left_ticks, avg_right_ticks;
|
23 |
float ang_vl, ang_vr, lin_vl, lin_vr;
|
24 |
float robot_v, robot_w;
|
25 |
float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32;
|
26 |
// figure out the encoder ticks during the time
|
27 |
encoder_readings scout_enc = encoders->query(); |
28 |
motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last; |
29 |
motor_fr_ticks_iter = scout_enc.fr_ticks - motor_fr_ticks_last; |
30 |
motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last; |
31 |
motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last; |
32 |
|
33 |
// update the
|
34 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
35 |
motor_fr_ticks_last = scout_enc.fr_ticks; |
36 |
motor_bl_ticks_last = scout_enc.bl_ticks; |
37 |
motor_br_ticks_last = scout_enc.br_ticks; |
38 |
|
39 |
avg_left_ticks = (float) motor_fl_ticks_iter;
|
40 |
avg_right_ticks = (float) motor_fr_ticks_iter;
|
41 |
// right now the back encoders does not work so uncomment the next
|
42 |
// two lines when the work!!!
|
43 |
/*
|
44 |
avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
|
45 |
avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
|
46 |
*/
|
47 |
|
48 |
// figure out the angular velocity in radians per second
|
49 |
ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK; |
50 |
ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK; |
51 |
|
52 |
// figure out the linear velosity of robot
|
53 |
lin_vl = ang_vl * WHEEL_R; |
54 |
lin_vr = ang_vr * WHEEL_R; |
55 |
|
56 |
// figure out how the robot speed and turning speed
|
57 |
robot_v = (lin_vl/2.0 + lin_vr/2.0); |
58 |
robot_w = (lin_vr - lin_vl)/WHEEL_B; |
59 |
|
60 |
// now use RK4 to integrade the velocities to get the the move in pos
|
61 |
k00 = robot_v * cos(scout_pos->theta); |
62 |
k01 = robot_v * sin(scout_pos->theta); |
63 |
k02 = robot_w; |
64 |
|
65 |
k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
|
66 |
k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
|
67 |
k12 = robot_w; |
68 |
|
69 |
k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
|
70 |
k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
|
71 |
k22 = robot_w; |
72 |
|
73 |
k30 = robot_v * cos(scout_pos->theta + loop_time * k22); |
74 |
k31 = robot_v * sin(scout_pos->theta + loop_time * k22); |
75 |
k32 = robot_w; |
76 |
|
77 |
scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30); |
78 |
scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31); |
79 |
scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32); |
80 |
|
81 |
|
82 |
} |
83 |
|
84 |
void Odometry_new::run()
|
85 |
{ |
86 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000); |
87 |
double last_time = Time::now().toSec();
|
88 |
double loop_time, current_time;
|
89 |
|
90 |
//Rate r(LOOP_RATE);
|
91 |
Duration r = Duration(LOOP_TIME); |
92 |
|
93 |
motors->set_sides(-50,50, MOTOR_ABSOLUTE); |
94 |
|
95 |
while(ok())
|
96 |
{ |
97 |
current_time = Time::now().toSec(); |
98 |
loop_time = current_time - last_time; |
99 |
last_time = current_time; |
100 |
|
101 |
get_position(loop_time); |
102 |
ROS_INFO("loop time %f", loop_time);
|
103 |
|
104 |
position.name = name; |
105 |
position.x = scout_pos->x; |
106 |
position.y = scout_pos->y; |
107 |
position.theta = scout_pos->theta; |
108 |
scout_position.publish(position); |
109 |
|
110 |
ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
|
111 |
|
112 |
r.sleep(); |
113 |
} |
114 |
} |