Project

General

Profile

Revision 34a60a3b

ID34a60a3b2ffaef82b1e09ae7402d4c1283165361
Parent a38fa869
Child d1ba04dc

Added by Priya over 4 years ago

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.

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