Revision f325b893 scout/libscout/src/MotorControl.cpp

View differences:

scout/libscout/src/MotorControl.cpp
153 153

  
154 154
    /* Set all the speeds (the booleans in the set_motors message determine
155 155
     * which ones will be used) */
156
    msg.fl_speed = (int) motor_fl_speed;
157
    msg.fr_speed = (int) motor_fr_speed;
158
    msg.bl_speed = (int) motor_bl_speed;
159
    msg.br_speed = (int) motor_br_speed;
156
    msg.fl_speed = trim_speed("FL", (int) motor_fl_speed);
157
    msg.fr_speed = trim_speed("FR", (int) motor_fr_speed);
158
    msg.bl_speed = trim_speed("BL", (int) motor_bl_speed);
159
    msg.br_speed = trim_speed("BR", (int) motor_br_speed);
160 160

  
161 161
    /* Publishes message to set_motors topic */
162 162
    set_motors_pub.publish(msg);
......
182 182
}
183 183

  
184 184
/**
185
 * Trims an absolute speed to stay within motor maximum range.
186
 * Uses ROS_WARN to warn the user if a speed has been trimmed.
187
 *
188
 * @param which_str A description of which motor, to be used in the warning
189
 *                  message (for example, "FL")
190
 * @param speed The requested speed to be set to
191
 * @return The requested speed, trimmed to stay within limits.
192
 */
193
int MotorControl::trim_speed(string which_str, int speed)
194
{
195
    int trimmed_speed = speed;
196
    if (speed > MAXSPEED_ABS)
197
    {
198
        trimmed_speed = (int) MAXSPEED_ABS;
199
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
200
            (int) MAXSPEED_ABS);
201
    }
202
    else if (speed < -MAXSPEED_ABS)
203
    {
204
        trimmed_speed = (int) -MAXSPEED_ABS;
205
        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
206
            (int) -MAXSPEED_ABS);
207
    }
208

  
209
    return trimmed_speed;
210
}
211

  
212
/**
185 213
 * @brief Query the current speeds of the motors
186 214
 *
187 215
 * Sends a request to the query_motors service which will reply with the
......
230 258
 **/
231 259
float MotorControl::rel_to_abs(float rel_speed, int units)
232 260
{
261
    ROS_INFO("Input: %f of units #%d.", rel_speed, units);
233 262
    switch(units)
234 263
    {
235 264
      case MOTOR_ABSOLUTE:/* Speed given as absolute */
......
237 266
      case MOTOR_PERCENT:/* Convert from percentage */
238 267
        return rel_speed * MAXSPEED_ABS / 100.0;
239 268
      case MOTOR_MMS:/* Convert from mm/s */
240
        return rel_speed * MAXSPEED_ABS * 100.0 / MAXSPEED_MPS;
269
        return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS);
241 270
      case MOTOR_CMS:/* Convert from cm/s */
242
        return rel_speed * MAXSPEED_ABS * 10.0 / MAXSPEED_MPS;
271
        return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS);
243 272
      default: /* The units aren't recognized */
244 273
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
245 274
        return rel_speed;
......
262 291
      case MOTOR_PERCENT:/* Convert from percentage */
263 292
        return abs_speed * 100.0 / MAXSPEED_ABS;
264 293
      case MOTOR_MMS:/* Convert from mm/s */
265
        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 100.0);
294
        return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS;
266 295
      case MOTOR_CMS:/* Convert from cm/s */
267
        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 10.0);
296
        return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS;
268 297
      default: /* The units aren't recognized */
269 298
        ROS_WARN("MotorControl::rel_to_abs used with default units.");
270 299
        return abs_speed;

Also available in: Unified diff