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