Project

General

Profile

Revision 34a60a3b

ID34a60a3b2ffaef82b1e09ae7402d4c1283165361
Parent a38fa869
Child d1ba04dc

Added by Priya about 11 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