commit f325b8937314ea7e1f7768e4b335be7fce269683
Author: Alex <azirbel@andrew.cmu.edu>
Date:   Sun Jan 6 18:50:48 2013 -0500

    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).

diff --git a/scout/libscout/src/MotorControl.cpp b/scout/libscout/src/MotorControl.cpp
index 0c2661d..107308b 100644
--- a/scout/libscout/src/MotorControl.cpp
+++ b/scout/libscout/src/MotorControl.cpp
@@ -153,10 +153,10 @@ void MotorControl::set_each(int which,
 
     /* Set all the speeds (the booleans in the set_motors message determine
      * which ones will be used) */
-    msg.fl_speed = (int) motor_fl_speed;
-    msg.fr_speed = (int) motor_fr_speed;
-    msg.bl_speed = (int) motor_bl_speed;
-    msg.br_speed = (int) motor_br_speed;
+    msg.fl_speed = trim_speed("FL", (int) motor_fl_speed);
+    msg.fr_speed = trim_speed("FR", (int) motor_fr_speed);
+    msg.bl_speed = trim_speed("BL", (int) motor_bl_speed);
+    msg.br_speed = trim_speed("BR", (int) motor_br_speed);
 
     /* Publishes message to set_motors topic */
     set_motors_pub.publish(msg);
@@ -182,6 +182,34 @@ void MotorControl::check_which_ok(int which)
 }
 
 /**
+ * Trims an absolute speed to stay within motor maximum range.
+ * Uses ROS_WARN to warn the user if a speed has been trimmed.
+ *
+ * @param which_str A description of which motor, to be used in the warning
+ *                  message (for example, "FL")
+ * @param speed The requested speed to be set to
+ * @return The requested speed, trimmed to stay within limits.
+ */
+int MotorControl::trim_speed(string which_str, int speed)
+{
+    int trimmed_speed = speed;
+    if (speed > MAXSPEED_ABS)
+    {
+        trimmed_speed = (int) MAXSPEED_ABS;
+        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
+            (int) MAXSPEED_ABS);
+    }
+    else if (speed < -MAXSPEED_ABS)
+    {
+        trimmed_speed = (int) -MAXSPEED_ABS;
+        ROS_WARN("Motor (%s) speed trimmed to %d.", which_str.c_str(),
+            (int) -MAXSPEED_ABS);
+    }
+
+    return trimmed_speed;
+}
+
+/**
  * @brief Query the current speeds of the motors
  *
  * Sends a request to the query_motors service which will reply with the
@@ -230,6 +258,7 @@ float MotorControl::query(int which)
  **/
 float MotorControl::rel_to_abs(float rel_speed, int units)
 {
+    ROS_INFO("Input: %f of units #%d.", rel_speed, units);
     switch(units)
     {
       case MOTOR_ABSOLUTE:/* Speed given as absolute */
@@ -237,9 +266,9 @@ float MotorControl::rel_to_abs(float rel_speed, int units)
       case MOTOR_PERCENT:/* Convert from percentage */
         return rel_speed * MAXSPEED_ABS / 100.0;
       case MOTOR_MMS:/* Convert from mm/s */
-        return rel_speed * MAXSPEED_ABS * 100.0 / MAXSPEED_MPS;
+        return rel_speed * MAXSPEED_ABS / (1000.0 * MAXSPEED_MPS);
       case MOTOR_CMS:/* Convert from cm/s */
-        return rel_speed * MAXSPEED_ABS * 10.0 / MAXSPEED_MPS;
+        return rel_speed * MAXSPEED_ABS / (100.0 * MAXSPEED_MPS);
       default: /* The units aren't recognized */
         ROS_WARN("MotorControl::rel_to_abs used with default units.");
         return rel_speed;
@@ -262,9 +291,9 @@ float MotorControl::abs_to_rel(float abs_speed, int units)
       case MOTOR_PERCENT:/* Convert from percentage */
         return abs_speed * 100.0 / MAXSPEED_ABS;
       case MOTOR_MMS:/* Convert from mm/s */
-        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 100.0);
+        return abs_speed * MAXSPEED_MPS * 1000.0 / MAXSPEED_ABS;
       case MOTOR_CMS:/* Convert from cm/s */
-        return abs_speed * MAXSPEED_MPS / (MAXSPEED_ABS * 10.0);
+        return abs_speed * MAXSPEED_MPS * 100.0 / MAXSPEED_ABS;
       default: /* The units aren't recognized */
         ROS_WARN("MotorControl::rel_to_abs used with default units.");
         return abs_speed;
diff --git a/scout/libscout/src/MotorControl.h b/scout/libscout/src/MotorControl.h
index f9f8537..1435566 100644
--- a/scout/libscout/src/MotorControl.h
+++ b/scout/libscout/src/MotorControl.h
@@ -101,6 +101,7 @@ class MotorControl
     private:
         /** Error if which sets a motor to both forward and backward */
         void check_which_ok(int which);
+        int trim_speed(std::string which_str, int speed);
 
         float rel_to_abs(float rel_speed, int units);
         float abs_to_rel(float abs_speed, int units);
