Statistics
| Branch: | Revision:

scoutos / scout / libscout / src / behaviors / Odometry.cpp @ d7e1b0d8

History | View | Annotate | Download (2.58 KB)

1

    
2
#include "Odometry.h"
3

    
4
using namespace std;
5

    
6
/** Set up the odometry node and prepare communcations over ROS */
7
Odometry::Odometry(string scoutname, Sensors* sensors)
8
    : Behavior(scoutname, "odometry", sensors)
9
{
10
    name = scoutname;
11
    scout_pos = new pos;
12
    motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
13
}
14

    
15
/** Set up the odometry node and prepare communcations over ROS */
16
    Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
17
: Behavior(scoutname, behavior_name, sensors)
18
{
19
    name = scoutname;
20
    scout_pos = new pos;
21
    motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
22
}
23

    
24
void Odometry::wait(float duration)
25
{
26
    ros::Rate r(WAIT_HZ);
27
    int ticks = int(duration * WAIT_HZ);
28
    for (int i = 0; i < ticks; i++)
29
    {
30
        get_position();
31
        spinOnce();
32
        r.sleep();
33
    }
34
}
35

    
36
/** Query encoders and estimate position based on encoder reading */
37
void Odometry::get_position()
38
{
39
    float left_dist, right_dist, total_dist, theta;
40

    
41
    encoder_readings scout_enc = encoders->query();
42
    motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
43
    motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
44
    motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
45
    motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
46

    
47
    // Get Left and Right distance
48
    left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
49
            ((float)(motor_bl_dist))/2.0);
50
    right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
51
            ((float)(motor_br_dist))/2.0);
52

    
53
    total_dist = (left_dist)/2.0 + (right_dist)/2.0;
54
    theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
55

    
56
    //Use negative theta because we measure theta from 0 on the
57
    //right to 180 on the left counter-clock-wise, but this is
58
    //negative theta in the coordinate frame.
59
    //Also, subtract the delta from y because positive y is down.
60
    scout_pos->x += total_dist*cos(-theta);
61
    scout_pos->y -= total_dist*sin(-theta);
62
    scout_pos->theta = fmod(theta, (float)(2*M_PI));
63

    
64
    //Save state for next time in.
65
    motor_fl_ticks = scout_enc.fl_ticks;
66
    motor_fr_ticks = scout_enc.fr_ticks;
67
    motor_bl_ticks = scout_enc.bl_ticks;
68
    motor_br_ticks = scout_enc.br_ticks;
69

    
70

    
71
    return;
72
}
73

    
74
void Odometry::run()
75
{
76
    scout_position = node.advertise< ::messages::ScoutPosition>(name+"/position", 1000); 
77

    
78
    while(ok())
79
    {
80
        get_position();
81

    
82
        position.name = name;
83
        position.x = scout_pos->x;
84
        position.y = scout_pos->y;
85
        position.theta = scout_pos->theta;
86
        scout_position.publish(position);
87
    }
88
}