Revision 34a60a3b
Added RungeKutta to the helper classes and changed the odometry to use that instead. Also created PaintboardControl files, changed the Makefile to compile all control classes and deleted Cliffsensor control becase it did not work.
scout/libscout/src/test_behaviors/Odometry_new.cpp | ||
---|---|---|
1 |
|
|
2 | 1 |
#include "Odometry_new.h" |
2 |
#include "../helper_classes/RungaKutta.h" |
|
3 | 3 |
|
4 | 4 |
using namespace std; |
5 | 5 |
|
... | ... | |
22 | 22 |
float avg_left_ticks, avg_right_ticks; |
23 | 23 |
float ang_vl, ang_vr, lin_vl, lin_vr; |
24 | 24 |
float robot_v, robot_w; |
25 |
float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32; |
|
25 |
|
|
26 | 26 |
// figure out the encoder ticks during the time |
27 | 27 |
encoder_readings scout_enc = encoders->query(); |
28 | 28 |
motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last; |
... | ... | |
30 | 30 |
motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last; |
31 | 31 |
motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last; |
32 | 32 |
|
33 |
// update the |
|
33 |
ROS_INFO("%d %d", motor_fl_ticks_iter, motor_fr_ticks_iter); |
|
34 |
|
|
35 |
// update the tick values |
|
34 | 36 |
motor_fl_ticks_last = scout_enc.fl_ticks; |
35 | 37 |
motor_fr_ticks_last = scout_enc.fr_ticks; |
36 | 38 |
motor_bl_ticks_last = scout_enc.bl_ticks; |
... | ... | |
58 | 60 |
robot_w = (lin_vr - lin_vl)/WHEEL_B; |
59 | 61 |
|
60 | 62 |
// 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 |
k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02); |
|
66 |
k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02); |
|
67 |
k12 = robot_w; |
|
68 |
|
|
69 |
k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12); |
|
70 |
k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12); |
|
71 |
k22 = robot_w; |
|
72 |
|
|
73 |
k30 = robot_v * cos(scout_pos->theta + loop_time * k22); |
|
74 |
k31 = robot_v * sin(scout_pos->theta + loop_time * k22); |
|
75 |
k32 = robot_w; |
|
76 |
|
|
77 |
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); |
|
63 |
double state_temp[] = {scout_pos->x, scout_pos->y, scout_pos->theta, |
|
64 |
robot_v, robot_w}; |
|
65 |
vector<double> state (state_temp, state_temp+5); |
|
80 | 66 |
|
67 |
vector<double> new_state = RungaKutta::rk4(state, RungaKutta::diff_drive_robot, loop_time); |
|
81 | 68 |
|
69 |
scout_pos->x = new_state[0]; |
|
70 |
scout_pos->y = new_state[1]; |
|
71 |
scout_pos->theta = new_state[2]; |
|
82 | 72 |
} |
83 | 73 |
|
84 | 74 |
void Odometry_new::run() |
... | ... | |
87 | 77 |
double last_time = Time::now().toSec(); |
88 | 78 |
double loop_time, current_time; |
89 | 79 |
|
80 |
// Reset encoders. |
|
81 |
encoders->reset(); |
|
82 |
|
|
90 | 83 |
//Rate r(LOOP_RATE); |
91 | 84 |
Duration r = Duration(LOOP_TIME); |
92 | 85 |
|
Also available in: Unified diff