Revision 36701fcd

View differences:

scout/libscout/src/test_behaviors/Odometry_new.cpp
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
  motor_fl_ticks_last = motor_fr_ticks_last = 0;
13
  motor_bl_ticks_last = motor_br_ticks_last = 0;
14
}
15

  
16

  
17
/** Query encoders and integrate using RK4 to estimate position */
18
void Odometry_new::get_position(double loop_time)
19
{
20
  float avg_left_ticks, avg_right_ticks;
21
  float ang_vl, ang_vr, lin_vl, lin_vr;
22
  float robot_v, robot_w;
23
  float k00, k01, k02, k10, k11, k12, k20, k21, k22, k30, k31, k32;
24
  // figure out the encoder ticks during the time
25
  encoder_readings scout_enc = encoders->query();
26
  motor_fl_ticks_iter = scout_enc.fl_ticks - motor_fl_ticks_last;
27
  motor_fr_ticks_iter = scout_enc.fr_ticks - motor_fr_ticks_last;
28
  motor_bl_ticks_iter = scout_enc.bl_ticks - motor_bl_ticks_last;
29
  motor_br_ticks_iter = scout_enc.br_ticks - motor_br_ticks_last;
30
  
31

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

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

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

  
62
  k20 = robot_v * cos(scout_pos->theta + LOOP_TIME/2*k12);
63
  k21 = robot_v * sin(scout_pos->theta + LOOP_TIME/2*k12);
64
  k22 = robot_w;
65

  
66
  k30 = robot_v * cos(scout_pos->theta + LOOP_TIME * k22);
67
  k31 = robot_v * sin(scout_pos->theta + LOOP_TIME * k22);
68
  k32 = robot_w;
69

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

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

  
80
}
81

  
82
void Odometry_new::run()
83
{
84
  scout_position = node.advertise< ::messages::ScoutPosition>(name+"/posn", 1000);
85
  double last_time = Time::now().toSec();
86
  double loop_time, current_time;
87
  motors->set_sides(50,50, MOTOR_ABSOLUTE);
88
  while(ok())
89
  {
90
    //Rate r(LOOP_RATE); 
91
    Duration r = Duration(LOOP_TIME);
92
    current_time = Time::now().toSec();
93
    loop_time = current_time - last_time;
94
    last_time = current_time;
95
    get_position(loop_time);
96
    position.name = name;
97
    position.x = scout_pos->x;
98
    position.y = scout_pos->y;
99
    position.theta = scout_pos->theta;
100
    scout_position.publish(position);
101
    ROS_INFO("scout is at %f %f theta: %f", position.x, position.y, position.theta);
102
    r.sleep();
103
  }
104

  
105
}
scout/libscout/src/test_behaviors/Odometry_new.h
1
// a smarter Odometry the use RK4 integration
2

  
3
#ifndef _ODOMETRY_NEW_H_
4
#define _ODOMETRY_NEW_H_
5

  
6
#include "../Behavior.h"
7
#include "../Sensors.h"
8
#include "messages/ScoutPosition.h"
9

  
10
#define WHEEL_R 1 //this is a virtual unit now... use real values
11
#define WHEEL_B 1  // this is a virtual unit now... use real values
12
#define TICKS_PER_REV 48
13
#define PI 3.1415926
14
#define RAD_PER_TICK ((TICKS_PER_REV)/360.0 * PI / 180.0)
15
#define LOOP_RATE 10 // Hz, integrate 10 times per second
16
#define LOOP_TIME (1.0/LOOP_RATE) // secs
17

  
18
typedef struct{
19
  float x;
20
  float y;
21
  float theta;
22
} posi;
23

  
24
class Odometry_new : Behavior{
25

  
26
  public:
27

  
28
    /** Set up the odometry node and prepare communications over ROS */
29
    Odometry_new(std::string scoutname, Sensors* sensors);
30

  
31
    /** Query encoders and estimate position based on encoder reading */
32
    void get_position(double loop_time);
33

  
34
    /** Gets scout position and prints to screen */
35
    void run();
36
    
37
  private:
38

  
39
    /** ROS publisher and client declaration */
40
    ros::NodeHandle node;
41
    ros::Publisher scout_position;
42
    ros::ServiceClient query_encoders_client;
43
    messages::ScoutPosition position;
44

  
45
    std::string name;
46

  
47
    // FIXME: not sure whether these would overflow....
48
    int motor_fl_ticks_iter;
49
    int motor_fr_ticks_iter;
50
    int motor_bl_ticks_iter;
51
    int motor_br_ticks_iter;
52
    
53
    int motor_fl_ticks_last;
54
    int motor_fr_ticks_last;
55
    int motor_bl_ticks_last;
56
    int motor_br_ticks_last;
57
    int scout_theta;
58

  
59
    posi* scout_pos;
60

  
61
};
62

  
63
#endif

Also available in: Unified diff