Revision d7e1b0d8

View differences:

scout/libscout/src/Behavior.cpp
118 118
void Behavior::wait(float duration)
119 119
{
120 120
    ros::Rate r(WAIT_HZ);
121
    int ticks = int(duration * 100);
121
    int ticks = int(duration * WAIT_HZ);
122 122
    for (int i = 0; i < ticks; i++)
123 123
    {
124 124
        spinOnce();
scout/libscout/src/Behavior.h
103 103
        static void spin();
104 104
        static void spinOnce();
105 105

  
106
        static void wait(float duration);
106
        virtual void wait(float duration);
107 107
};
108 108

  
109 109
#endif
scout/libscout/src/behaviors/Odometry.cpp
7 7
Odometry::Odometry(string scoutname, Sensors* sensors)
8 8
    : Behavior(scoutname, "odometry", sensors)
9 9
{
10
  name = scoutname;
11
  scout_pos = new pos;
12
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
10
    name = scoutname;
11
    scout_pos = new pos;
12
    motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
13 13
}
14 14

  
15 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)
16
    Odometry::Odometry(string scoutname, string behavior_name, Sensors* sensors)
17
: Behavior(scoutname, behavior_name, sensors)
18 18
{
19
  name = scoutname;
20
  scout_pos = new pos;
21
  motor_fl_ticks = motor_fr_ticks = motor_bl_ticks = motor_br_ticks = 0;
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
    }
22 34
}
23 35

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

  
29
  encoder_readings scout_enc = encoders->query();
30
  motor_fl_dist = scout_enc.fl_ticks - motor_fl_ticks;
31
  motor_fr_dist = scout_enc.fr_ticks - motor_fr_ticks;
32
  motor_bl_dist = scout_enc.bl_ticks - motor_bl_ticks;
33
  motor_br_dist = scout_enc.br_ticks - motor_br_ticks;
34

  
35
  // Get Left and Right distance
36
  left_dist = DIST_PER_TICK*(((float)(motor_fl_dist))/2.0 +
37
                             ((float)(motor_bl_dist))/2.0);
38
  right_dist = DIST_PER_TICK*(((float)(motor_fr_dist))/2.0 + 
39
                              ((float)(motor_br_dist))/2.0);
40

  
41
  total_dist = (left_dist)/2.0 + (right_dist)/2.0;
42
  theta = scout_pos->theta + atan2(right_dist - left_dist, WHEEL_BASE);
43

  
44
  //Use negative theta because we measure theta from 0 on the
45
  //right to 180 on the left counter-clock-wise, but this is
46
  //negative theta in the coordinate frame.
47
  //Also, subtract the delta from y because positive y is down.
48
  scout_pos->x += total_dist*cos(-theta);
49
  scout_pos->y -= total_dist*sin(-theta);
50
  scout_pos->theta = fmod(theta, (float)(2*M_PI));
51

  
52
  //Save state for next time in.
53
  motor_fl_ticks = scout_enc.fl_ticks;
54
  motor_fr_ticks = scout_enc.fr_ticks;
55
  motor_bl_ticks = scout_enc.bl_ticks;
56
  motor_br_ticks = scout_enc.br_ticks;
57

  
58

  
59
 return;
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;
60 72
}
61 73

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

  
66
  while(ok())
67
  {
68
    get_position();
69

  
70
    position.name = name;
71
    position.x = scout_pos->x;
72
    position.y = scout_pos->y;
73
    position.theta = scout_pos->theta;
74
    scout_position.publish(position);
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
    }
76 88
}
scout/libscout/src/behaviors/Odometry.h
30 30

  
31 31
    protected:
32 32

  
33
        virtual void wait(float duration);
33 34
        pos* scout_pos;
34 35

  
35 36
    private:

Also available in: Unified diff