Revision f325b893
Added conversions abs_to_rel and rel_to_abs.
Also, added warnings after these conversions in the case that a requested speed has to be cropped.
Tested against a temporary behavior which set the speeds (now removed since it's very easy to write such a test behavior).
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; |
scout/libscout/src/MotorControl.h | ||
---|---|---|
101 | 101 |
private: |
102 | 102 |
/** Error if which sets a motor to both forward and backward */ |
103 | 103 |
void check_which_ok(int which); |
104 |
int trim_speed(std::string which_str, int speed); |
|
104 | 105 |
|
105 | 106 |
float rel_to_abs(float rel_speed, int units); |
106 | 107 |
float abs_to_rel(float abs_speed, int units); |
Also available in: Unified diff