Revision beaf9201
Fixes to the Odometery behavior.
scout/libscout/src/test_behaviors/Odometry_new.cpp | ||
---|---|---|
9 | 9 |
{ |
10 | 10 |
name = scoutname; |
11 | 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; |
|
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; |
|
14 | 16 |
} |
15 | 17 |
|
16 | 18 |
|
... | ... | |
55 | 57 |
k01 = robot_v * sin(scout_pos->theta); |
56 | 58 |
k02 = robot_w; |
57 | 59 |
|
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 |
k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
|
|
61 |
k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
|
|
60 | 62 |
k12 = robot_w; |
61 | 63 |
|
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 |
k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
|
|
65 |
k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
|
|
64 | 66 |
k22 = robot_w; |
65 | 67 |
|
66 |
k30 = robot_v * cos(scout_pos->theta + LOOP_TIME * k22);
|
|
67 |
k31 = robot_v * sin(scout_pos->theta + LOOP_TIME * k22);
|
|
68 |
k30 = robot_v * cos(scout_pos->theta + loop_time * k22);
|
|
69 |
k31 = robot_v * sin(scout_pos->theta + loop_time * k22);
|
|
68 | 70 |
k32 = robot_w; |
69 | 71 |
|
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);
|
|
72 |
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);
|
|
73 | 75 |
|
74 | 76 |
// update the |
75 | 77 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
... | ... | |
84 | 86 |
scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000); |
85 | 87 |
double last_time = Time::now().toSec(); |
86 | 88 |
double loop_time, current_time; |
89 |
|
|
90 |
//Rate r(LOOP_RATE); |
|
91 |
Duration r = Duration(LOOP_TIME); |
|
92 |
|
|
87 | 93 |
motors->set_sides(50,50, MOTOR_ABSOLUTE); |
94 |
|
|
88 | 95 |
while(ok()) |
89 | 96 |
{ |
90 |
//Rate r(LOOP_RATE); |
|
91 |
Duration r = Duration(LOOP_TIME); |
|
92 | 97 |
current_time = Time::now().toSec(); |
93 | 98 |
loop_time = current_time - last_time; |
94 | 99 |
last_time = current_time; |
100 |
|
|
95 | 101 |
get_position(loop_time); |
102 |
ROS_INFO("loop time %f", loop_time); |
|
103 |
|
|
96 | 104 |
position.name = name; |
97 | 105 |
position.x = scout_pos->x; |
98 | 106 |
position.y = scout_pos->y; |
99 | 107 |
position.theta = scout_pos->theta; |
100 | 108 |
scout_position.publish(position); |
109 |
|
|
101 | 110 |
ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta); |
111 |
|
|
102 | 112 |
r.sleep(); |
103 | 113 |
} |
104 |
|
|
105 | 114 |
} |
Also available in: Unified diff