scoutos / scout / libscout / src / test_behaviors / Odometry_new.cpp @ beaf9201
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 | scout_pos = new posi;
|
||
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 | |||
34 | avg_left_ticks = (float) motor_fl_ticks_iter;
|
||
35 | avg_right_ticks = (float) motor_fr_ticks_iter;
|
||
36 | // right now the back encoders does not work so uncomment the next
|
||
37 | // two lines when the work!!!
|
||
38 | /*
|
||
39 | avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
|
||
40 | avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
|
||
41 | */
|
||
42 | |||
43 | // figure out the angular velocity in radians per second
|
||
44 | ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK; |
||
45 | ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK; |
||
46 | |||
47 | // figure out the linear velosity of robot
|
||
48 | lin_vl = ang_vl * WHEEL_R; |
||
49 | lin_vr = ang_vr * WHEEL_R; |
||
50 | |||
51 | // figure out how the robot speed and turning speed
|
||
52 | robot_v = (lin_vl/2.0 + lin_vr/2.0); |
||
53 | robot_w = (lin_vr - lin_vl)/WHEEL_B; |
||
54 | |||
55 | // now use RK4 to integrade the velocities to get the the move in pos
|
||
56 | k00 = robot_v * cos(scout_pos->theta); |
||
57 | k01 = robot_v * sin(scout_pos->theta); |
||
58 | k02 = robot_w; |
||
59 | |||
60 | beaf9201 | Colony Scout | k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
|
61 | k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
|
||
62 | 36701fcd | Yuyang Guo | k12 = robot_w; |
63 | |||
64 | beaf9201 | Colony Scout | k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
|
65 | k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
|
||
66 | 36701fcd | Yuyang Guo | k22 = robot_w; |
67 | |||
68 | beaf9201 | Colony Scout | k30 = robot_v * cos(scout_pos->theta + loop_time * k22); |
69 | k31 = robot_v * sin(scout_pos->theta + loop_time * k22); |
||
70 | 36701fcd | Yuyang Guo | k32 = robot_w; |
71 | |||
72 | beaf9201 | Colony Scout | scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30); |
73 | scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31); |
||
74 | scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32); |
||
75 | 36701fcd | Yuyang Guo | |
76 | // update the
|
||
77 | motor_fl_ticks_last = scout_enc.fl_ticks; |
||
78 | motor_fr_ticks_last = scout_enc.fr_ticks; |
||
79 | motor_bl_ticks_last = scout_enc.bl_ticks; |
||
80 | motor_br_ticks_last = scout_enc.br_ticks; |
||
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 | 36701fcd | Yuyang Guo | 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 | } |