Project

General

Profile

Revision 9f547ef7

ID9f547ef719e5b32f52210cdb56bc7cae9eb16c7d
Parent 95bede66
Child 24145262, 0e6831f5

Added by Alex Zirbel over 7 years ago

Added scoutsim support for encoders.

To see the query_encoders.srv (used for accessing this data) in action, use: rosrun libscout test_encoders. You will see a stream of encoder data.

View differences:

scout/scoutsim/src/scout.cpp
56 56
namespace scoutsim
57 57
{
58 58
    Scout::Scout(const ros::NodeHandle& nh,
59
            const wxImage& scout_image,
60
            const Vector2& pos,
61
            float orient)
59
                 const wxImage& scout_image,
60
                 const Vector2& pos,
61
                 float orient)
62 62
        : node (nh)
63 63
          , scout_image(scout_image)
64 64
          , pos(pos)
65 65
          , orient(orient)
66
          , lin_vel(0.0)
67
          , ang_vel(0.0)
66
          , motor_fl_speed(0)
67
          , motor_fr_speed(0)
68
          , motor_bl_speed(0)
69
          , motor_br_speed(0)
70
          , fl_ticks(0)
71
          , fr_ticks(0)
72
          , bl_ticks(0)
73
          , br_ticks(0)
68 74
          , pen_on(true)
69 75
          , pen(wxColour(DEFAULT_PEN_R, DEFAULT_PEN_G, DEFAULT_PEN_B))
70 76
    {
......
78 84
        set_pen_srv = node.advertiseService("set_pen",
79 85
                                            &Scout::setPenCallback,
80 86
                                            this);
87
        query_encoders_srv =
88
            node.advertiseService("query_encoders",
89
                                  &Scout::query_encoders_callback,
90
                                  this);
81 91

  
82 92
        meter = scout.GetHeight();
83 93
    }
......
108 118
            motor_br_speed = msg->br_speed;
109 119
        }
110 120

  
111
        // Assume that the two motors on the same side will be set to
112
        // roughly the same speed. Does not account for slip conditions
113
        // when they are set to different speeds.
114
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
115
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
116

  
117
        // Set the linear and angular speeds
118
        lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
119
        ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
120 121
    }
121 122

  
122 123
    float Scout::getSonar(float angle)
......
144 145
        return true;
145 146
    }
146 147

  
148
    bool Scout::query_encoders_callback(encoders::query_encoders::Request&,
149
                                        encoders::query_encoders::Response& res)
150
    {
151
        res.fl_distance = fl_ticks;
152
        res.fr_distance = fr_ticks;
153
        res.bl_distance = bl_ticks;
154
        res.br_distance = br_ticks;
155

  
156
        return true;
157
    }
158

  
147 159
    void Scout::update(double dt, wxMemoryDC& path_dc,
148 160
                        const wxImage& path_image, wxColour background_color,
149 161
                        float canvas_width, float canvas_height)
150 162
    {
163
        // Assume that the two motors on the same side will be set to
164
        // roughly the same speed. Does not account for slip conditions
165
        // when they are set to different speeds.
166
        float l_speed = (float (motor_fl_speed + motor_bl_speed)) / 2;
167
        float r_speed = (float (motor_fr_speed + motor_br_speed)) / 2;
168

  
169
        // Set the linear and angular speeds
170
        float lin_vel = SPEED_SCALE_FACTOR * (l_speed + r_speed) / 2;
171
        float ang_vel = SPEED_SCALE_FACTOR * (r_speed - l_speed);
172

  
151 173
        Vector2 old_pos = pos;
152 174

  
175
        // Update encoders
176
        /// @todo replace
177
        fl_ticks += (unsigned int) motor_fl_speed;
178
        fr_ticks += (unsigned int) motor_fr_speed;
179
        bl_ticks += (unsigned int) motor_bl_speed;
180
        br_ticks += (unsigned int) motor_br_speed;
181

  
153 182
        orient = fmod(orient + ang_vel * dt, 2*PI);
154 183
        pos.x += sin(orient + PI/2.0) * lin_vel * dt;
155 184
        pos.y += cos(orient + PI/2.0) * lin_vel * dt;

Also available in: Unified diff