Statistics
| Branch: | Revision:

root / scout / libscout / src / test_behaviors / Odometry_new.cpp @ 34a60a3b

History | View | Annotate | Download (3.29 KB)

1
#include "Odometry_new.h"
2
#include "../helper_classes/RungaKutta.h"
3

    
4
using namespace std;
5

    
6
/** Set up the odometry node and prepare communications over ROS */
7
Odometry_new::Odometry_new(string scoutname, Sensors* sensors):
8
                Behavior(scoutname, "odometry", sensors)
9
{
10
  name = scoutname;
11
  scout_pos = new pos;
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;
16
}
17

    
18

    
19
/** Query encoders and integrate using RK4 to estimate position */
20
void Odometry_new::get_position(double loop_time)
21
{
22
  float avg_left_ticks, avg_right_ticks;
23
  float ang_vl, ang_vr, lin_vl, lin_vr;
24
  float robot_v, robot_w;
25

    
26
  // figure out the encoder ticks during the time
27
  encoder_readings scout_enc = encoders->query();
28
  motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last;
29
  motor_fr_ticks_iter = scout_enc.fr_ticks - motor_fr_ticks_last;
30
  motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last;
31
  motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last;
32
  
33
  ROS_INFO("%d  %d", motor_fl_ticks_iter, motor_fr_ticks_iter);
34

    
35
  // update the tick values
36
  motor_fl_ticks_last = scout_enc.fl_ticks;
37
  motor_fr_ticks_last = scout_enc.fr_ticks;
38
  motor_bl_ticks_last = scout_enc.bl_ticks;
39
  motor_br_ticks_last = scout_enc.br_ticks;
40

    
41
  avg_left_ticks = (float) motor_fl_ticks_iter;
42
  avg_right_ticks = (float) motor_fr_ticks_iter;
43
  // right now the back encoders does not work so uncomment the next
44
  // two lines when the work!!!
45
  /*
46
  avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
47
  avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
48
  */
49
  
50
  // figure out the angular velocity in radians per second
51
  ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK;
52
  ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK;
53

    
54
  // figure out the linear velosity of robot
55
  lin_vl = ang_vl * WHEEL_R;
56
  lin_vr = ang_vr * WHEEL_R;
57
  
58
  // figure out how the robot speed and turning speed
59
  robot_v = (lin_vl/2.0 + lin_vr/2.0);
60
  robot_w = (lin_vr - lin_vl)/WHEEL_B;
61

    
62
  // now use RK4 to integrade the velocities to get the the move in pos
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);
66

    
67
  vector<double> new_state = RungaKutta::rk4(state, RungaKutta::diff_drive_robot, loop_time);
68

    
69
  scout_pos->x = new_state[0];
70
  scout_pos->y = new_state[1];
71
  scout_pos->theta = new_state[2];
72
}
73

    
74
void Odometry_new::run()
75
{
76
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000);
77
  double last_time = Time::now().toSec();
78
  double loop_time, current_time;
79

    
80
  // Reset encoders.
81
  encoders->reset();
82

    
83
  //Rate r(LOOP_RATE); 
84
  Duration r = Duration(LOOP_TIME);
85

    
86
  motors->set_sides(-50,50, MOTOR_ABSOLUTE);
87

    
88
  while(ok())
89
  {
90
    current_time = Time::now().toSec();
91
    loop_time = current_time - last_time;
92
    last_time = current_time;
93

    
94
    get_position(loop_time);
95
    ROS_INFO("loop time %f", loop_time);
96

    
97
    position.name = name;
98
    position.x = scout_pos->x;
99
    position.y = scout_pos->y;
100
    position.theta = scout_pos->theta;
101
    scout_position.publish(position);
102

    
103
    ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
104

    
105
    r.sleep();
106
  }
107
}