Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / test_behaviors / Odometry_new.cpp @ beaf9201

History | View | Annotate | Download (3.53 KB)

1

    
2
#include "Odometry_new.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 posi;
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
  float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32;
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

    
34
  avg_left_ticks = (float) motor_fl_ticks_iter;
35
  avg_right_ticks = (float) motor_fr_ticks_iter;
36
  // right now the back encoders does not work so uncomment the next
37
  // two lines when the work!!!
38
  /*
39
  avg_left_ticks = motor_fl_ticks_iter/(2.0) + motor_bl_ticks_iter/(2.0);
40
  avg_right_ticks = motor_fr_ticks_iter/(2.0) + motor_br_ticks_iter/(2.0);
41
  */
42
  
43
  // figure out the angular velocity in radians per second
44
  ang_vl = (avg_left_ticks/loop_time) * RAD_PER_TICK;
45
  ang_vr = (avg_right_ticks/loop_time) * RAD_PER_TICK;
46

    
47
  // figure out the linear velosity of robot
48
  lin_vl = ang_vl * WHEEL_R;
49
  lin_vr = ang_vr * WHEEL_R;
50
  
51
  // figure out how the robot speed and turning speed
52
  robot_v = (lin_vl/2.0 + lin_vr/2.0);
53
  robot_w = (lin_vr - lin_vl)/WHEEL_B;
54

    
55
  // now use RK4 to integrade the velocities to get the the move in pos
56
  k00 = robot_v * cos(scout_pos->theta);
57
  k01 = robot_v * sin(scout_pos->theta);
58
  k02 = robot_w;
59
  
60
  k10 = robot_v * cos(scout_pos->theta + loop_time/2*k02);
61
  k11 = robot_v * sin(scout_pos->theta + loop_time/2*k02);
62
  k12 = robot_w;
63

    
64
  k20 = robot_v * cos(scout_pos->theta + loop_time/2*k12);
65
  k21 = robot_v * sin(scout_pos->theta + loop_time/2*k12);
66
  k22 = robot_w;
67

    
68
  k30 = robot_v * cos(scout_pos->theta + loop_time * k22);
69
  k31 = robot_v * sin(scout_pos->theta + loop_time * k22);
70
  k32 = robot_w;
71

    
72
  scout_pos->x += loop_time/6 * (k00+2*(k10+k20)+k30);
73
  scout_pos->y += loop_time/6 * (k01+2*(k11+k21)+k31);
74
  scout_pos->theta += loop_time/6 * (k02+2*(k12+k22)+k32);
75

    
76
  // update the 
77
  motor_fl_ticks_last = scout_enc.fl_ticks;
78
  motor_fr_ticks_last = scout_enc.fr_ticks;
79
  motor_bl_ticks_last = scout_enc.bl_ticks;
80
  motor_br_ticks_last = scout_enc.br_ticks;
81

    
82
}
83

    
84
void Odometry_new::run()
85
{
86
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000);
87
  double last_time = Time::now().toSec();
88
  double loop_time, current_time;
89

    
90
  //Rate r(LOOP_RATE); 
91
  Duration r = Duration(LOOP_TIME);
92

    
93
  motors->set_sides(50,50, MOTOR_ABSOLUTE);
94

    
95
  while(ok())
96
  {
97
    current_time = Time::now().toSec();
98
    loop_time = current_time - last_time;
99
    last_time = current_time;
100

    
101
    get_position(loop_time);
102
    ROS_INFO("loop time %f", loop_time);
103

    
104
    position.name = name;
105
    position.x = scout_pos->x;
106
    position.y = scout_pos->y;
107
    position.theta = scout_pos->theta;
108
    scout_position.publish(position);
109

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

    
112
    r.sleep();
113
  }
114
}