Revision 34a60a3b scout/libscout/src/test_behaviors/Odometry_new.cpp

View differences:

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