scoutos / scout / libscout / src / test_behaviors / Odometry_new.cpp @ 8ae3771a
History | View | Annotate | Download (3.29 KB)
1 |
#include "Odometry_new.h" |
---|---|
2 |
#include "../helper_classes/RungaKutta.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 |
|
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 |
ROS_INFO("%d %d", motor_fl_ticks_iter, motor_fr_ticks_iter);
|
34 |
|
35 |
// update the tick values
|
36 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
37 |
motor_fr_ticks_last = scout_enc.fr_ticks; |
38 |
motor_bl_ticks_last = scout_enc.bl_ticks; |
39 |
motor_br_ticks_last = scout_enc.br_ticks; |
40 |
|
41 |
avg_left_ticks = (float) motor_fl_ticks_iter;
|
42 |
avg_right_ticks = (float) motor_fr_ticks_iter;
|
43 |
// right now the back encoders does not work so uncomment the next
|
44 |
// two lines when the work!!!
|
45 |
/*
|
46 |
avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
|
47 |
avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
|
48 |
*/
|
49 |
|
50 |
// figure out the angular velocity in radians per second
|
51 |
ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK; |
52 |
ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK; |
53 |
|
54 |
// figure out the linear velosity of robot
|
55 |
lin_vl = ang_vl * WHEEL_R; |
56 |
lin_vr = ang_vr * WHEEL_R; |
57 |
|
58 |
// figure out how the robot speed and turning speed
|
59 |
robot_v = (lin_vl/2.0 + lin_vr/2.0); |
60 |
robot_w = (lin_vr - lin_vl)/WHEEL_B; |
61 |
|
62 |
// now use RK4 to integrade the velocities to get the the move in pos
|
63 |
double state_temp[] = {scout_pos->x, scout_pos->y, scout_pos->theta,
|
64 |
robot_v, robot_w}; |
65 |
vector<double> state (state_temp, state_temp+5); |
66 |
|
67 |
vector<double> new_state = RungaKutta::rk4(state, RungaKutta::diff_drive_robot, loop_time);
|
68 |
|
69 |
scout_pos->x = new_state[0];
|
70 |
scout_pos->y = new_state[1];
|
71 |
scout_pos->theta = new_state[2];
|
72 |
} |
73 |
|
74 |
void Odometry_new::run()
|
75 |
{ |
76 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000); |
77 |
double last_time = Time::now().toSec();
|
78 |
double loop_time, current_time;
|
79 |
|
80 |
// Reset encoders.
|
81 |
encoders->reset(); |
82 |
|
83 |
//Rate r(LOOP_RATE);
|
84 |
Duration r = Duration(LOOP_TIME); |
85 |
|
86 |
motors->set_sides(-50,50, MOTOR_ABSOLUTE); |
87 |
|
88 |
while(ok())
|
89 |
{ |
90 |
current_time = Time::now().toSec(); |
91 |
loop_time = current_time - last_time; |
92 |
last_time = current_time; |
93 |
|
94 |
get_position(loop_time); |
95 |
ROS_INFO("loop time %f", loop_time);
|
96 |
|
97 |
position.name = name; |
98 |
position.x = scout_pos->x; |
99 |
position.y = scout_pos->y; |
100 |
position.theta = scout_pos->theta; |
101 |
scout_position.publish(position); |
102 |
|
103 |
ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
|
104 |
|
105 |
r.sleep(); |
106 |
} |
107 |
} |