root / scout / libscout / src / test_behaviors / Odometry_new.cpp @ b9e59a3c
History | View | Annotate | Download (3.53 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 | bb64f5e5 | Priya | scout_pos = new pos;
|
12 | beaf9201 | Colony Scout | |
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 | 36701fcd | Yuyang Guo | } |
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 | 259aaff8 | Colony Scout | // 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 | 36701fcd | Yuyang Guo | |
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 | beaf9201 | Colony Scout | k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
|
66 | k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
|
||
67 | 36701fcd | Yuyang Guo | k12 = robot_w; |
68 | |||
69 | beaf9201 | Colony Scout | k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
|
70 | k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
|
||
71 | 36701fcd | Yuyang Guo | k22 = robot_w; |
72 | |||
73 | beaf9201 | Colony Scout | k30 = robot_v * cos(scout_pos->theta + loop_time * k22); |
74 | k31 = robot_v * sin(scout_pos->theta + loop_time * k22); |
||
75 | 36701fcd | Yuyang Guo | k32 = robot_w; |
76 | |||
77 | beaf9201 | Colony Scout | 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 | 36701fcd | Yuyang Guo | |
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 | beaf9201 | Colony Scout | |
90 | //Rate r(LOOP_RATE);
|
||
91 | Duration r = Duration(LOOP_TIME); |
||
92 | |||
93 | 259aaff8 | Colony Scout | motors->set_sides(-50,50, MOTOR_ABSOLUTE); |
94 | beaf9201 | Colony Scout | |
95 | 36701fcd | Yuyang Guo | while(ok())
|
96 | { |
||
97 | current_time = Time::now().toSec(); |
||
98 | loop_time = current_time - last_time; |
||
99 | last_time = current_time; |
||
100 | beaf9201 | Colony Scout | |
101 | 36701fcd | Yuyang Guo | get_position(loop_time); |
102 | beaf9201 | Colony Scout | ROS_INFO("loop time %f", loop_time);
|
103 | |||
104 | 36701fcd | Yuyang Guo | 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 | beaf9201 | Colony Scout | |
110 | 36701fcd | Yuyang Guo | ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
|
111 | beaf9201 | Colony Scout | |
112 | 36701fcd | Yuyang Guo | r.sleep(); |
113 | } |
||
114 | } |