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 |
} |
scout/libscout/src/test_behaviors/Odometry_new.h | ||
---|---|---|
7 | 7 |
#include "../Sensors.h" |
8 | 8 |
#include "messages/ScoutPosition.h" |
9 | 9 |
|
10 |
#define WHEEL_R 1 //this is a virtual unit now... use real values
|
|
11 |
#define WHEEL_B 1 // this is a virtual unit now... use real values
|
|
10 |
#define WHEEL_R 0.025 //this is a virtual unit now... use real values
|
|
11 |
#define WHEEL_B 0.105 // this is a virtual unit now... use real values
|
|
12 | 12 |
#define TICKS_PER_REV 48 |
13 |
#define PI 3.1415926 |
|
14 |
#define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0) |
|
13 |
#define RAD_PER_TICK (2*M_PI/TICKS_PER_REV) |
|
15 | 14 |
#define LOOP_RATE 10 // Hz, integrate 10 times per second |
16 | 15 |
#define LOOP_TIME (1.0/LOOP_RATE) // secs |
17 | 16 |
|
Also available in: Unified diff