commit 2814387f48599dab8a99852093cc51629a14fb89
Author: Alex Zirbel <azirbel@andrew.cmu.edu>
Date:   Wed Nov 30 22:14:51 2011 -0500

    Updated motors code.
    
    Changed the definitions for the set_motors message, which forced a few
    implementation changes as well.
    
    Changed "libscout_node" to "node" and "n" to "node" in other packages.

diff --git a/scout/libscout/src/constants.h b/scout/libscout/src/constants.h
index c259fd7..779dcda 100644
--- a/scout/libscout/src/constants.h
+++ b/scout/libscout/src/constants.h
@@ -39,6 +39,9 @@
 
 #include "ros/ros.h"
 
+/* ROS defines */
+#define QUEUE_SIZE 10
+
 /* Libscout Defines */
 
 /* Init defines */
diff --git a/scout/libscout/src/libbuttons.cpp b/scout/libscout/src/libbuttons.cpp
index b55e825..90faf3f 100644
--- a/scout/libscout/src/libbuttons.cpp
+++ b/scout/libscout/src/libbuttons.cpp
@@ -41,7 +41,7 @@
 
 
 /* ROS node created in libscout.cpp */
-extern ros::NodeHandle libscout_node;
+extern ros::NodeHandle node;
 
 /** ROS publisher and client declaration */
 ros::Publisher button_event_publisher;
@@ -55,8 +55,8 @@ ros::ServiceClient query_buttons_client;
  */
 void libbuttons_init()
 {
-  button_event_publisher = libscout_node.advertise<buttons::button_event>("libbuttons", 10);
-  query_buttons_client = libscout_node.serviceClient<buttons::query_buttons>("libbuttons");
+  button_event_publisher = node.advertise<buttons::button_event>("libbuttons", 10);
+  query_buttons_client = node.serviceClient<buttons::query_buttons>("libbuttons");
 }
 
 /*!
diff --git a/scout/libscout/src/libheadlights.cpp b/scout/libscout/src/libheadlights.cpp
index 16bbd02..53f576c 100644
--- a/scout/libscout/src/libheadlights.cpp
+++ b/scout/libscout/src/libheadlights.cpp
@@ -38,7 +38,7 @@
 #include "libheadlights.h"
 
 /* ROS node created in libscout.cpp */
-extern ros::NodeHandle libscout_node;
+extern ros::NodeHandle node;
 
 /** ROS publisher and client declaration */
 ros::Publisher set_headlights_publisher;
@@ -51,7 +51,7 @@ ros::Publisher set_headlights_publisher;
 void libheadlights_init()
 {
   set_headlights_publisher =
-		libscout_node.advertise<headlights::set_headlights>("libheadlights", 10);
+		node.advertise<headlights::set_headlights>("libheadlights", 10);
 }
 
 /*!
diff --git a/scout/libscout/src/libmotors.cpp b/scout/libscout/src/libmotors.cpp
index 857d4b1..41d057b 100644
--- a/scout/libscout/src/libmotors.cpp
+++ b/scout/libscout/src/libmotors.cpp
@@ -37,123 +37,190 @@
 #include "libmotors.h"
 
 /* ROS node created in libscout.cpp */
-extern ros::NodeHandle libscout_node;
+extern ros::NodeHandle node;
 
 /** ROS publisher and client declaration */
 ros::Publisher set_motors_publisher;
 ros::ServiceClient query_motors_client;
 
-/*!
- * \brief Initialize the motors module of libscout.
+/**
+ * @brief Initialize the motors module of libscout.
  *
  * Initialize the libscout node as a publisher of set_motors and a client of
  * query_motors.
  */
 void libmotors_init()
 {
-  set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
-  query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
+    set_motors_publisher =
+        node.advertise<motors::set_motors>("set_motors", QUEUE_SIZE);
+    query_motors_client =
+        node.serviceClient<motors::query_motors>("query_motors");
 }
 
-/*!
- * \brief Set motor speeds
+/**
+ * @brief Sets all the speeds according to the specificiation of which motors
+ *        to set.
+ *
+ * @param which A bitmask of which motors need to be set.
+ * @param speed The value to set the motors to.
+ * @param units The units the speed is expressed in.
+ * @return Function status
+ */
+int motors_set(int which, float speed, char units)
+{
+    float speed_fl, speed_fr, speed_bl, speed_br;
+    speed_fl = speed_fr = speed_bl = speed_br = speed;
+
+    if(which & MOTOR_FL_REV)
+    {
+        speed_fl = -speed;
+    }
+    if(which & MOTOR_FR_REV)
+    {
+        speed_fr = -speed;
+    }
+    if(which & MOTOR_BL_REV)
+    {
+        speed_bl = -speed;
+    }
+    if(which & MOTOR_BR_REV)
+    {
+        speed_br = -speed;
+    }
+
+    return motors_set_each(which, speed_fl, speed_fr, speed_bl, speed_br,
+                           units);
+}
+
+/**
+ * @brief Sets the left and right sides of scout to different speeds.
+ *
+ * @param speed_l The speed of both left motors.
+ * @param speed_r The speed of both right motors.
+ * @param units The units to set to.
+ * @return Function status
+ */
+int motors_set_sides(float speed_l, float speed_r, char units)
+{
+    return motors_set_each(MOTOR_ALL, speed_l, speed_r, speed_l, speed_r,
+                               units);
+}
+
+/**
+ * @brief Set motor speeds
  * Sets the speeds of the motors as a percentage of top speed. Can selectively
  * select which motors to set, and which to remain at previous speed.
- * \param speed The speed the motors should be set to
- * \param which A bitmask of which motors should be set
- * \return Function status
+ *
+ * @param which A bitmask of which motors should be set.
+ * @param speed The speed the motors should be set to.
+ * @param units Optional units for the speeds.
+ * @return Function status
  */
-int motors_set(int speed, int which, char units=MOTOR_PERCENT)
+int motors_set_each(int which,
+                    float speed_fl, float speed_fr,
+                    float speed_bl, float speed_br,
+                    char units)
 {
-  /** \todo Set fields of the message based on params */
-  
-	motors::set_motors msg;
-	
-  if(!ros::ok())
-	{
-    return LIB_ERROR;
-  }
- 
-  /* Set the speed for each motor according to the which bitmask*/
-  
-  if(which & MOTOR_FL_REV)
-	{
-    msg.fl_speed = -1 * speed;
-  }
-  if(which & MOTOR_FR_REV)
-	{
-    msg.fr_speed = -1 * speed;
-  }
-  if(which & MOTOR_BL_REV)
-	{
-    msg.bl_speed = -1 * speed;
-  }
-  if(which & MOTOR_BR_REV)
-	{
-    msg.br_speed = -1 * speed;
-  }if(which & MOTOR_FL)
-	{
-    msg.fl_speed = speed;
-  }
-  if(which & MOTOR_FR)
-	{
-    msg.fr_speed = speed;
-  }
-  if(which & MOTOR_BL)
-	{
-    msg.bl_speed = speed;
-  }
-  if(which & MOTOR_BR)
-	{
-    msg.br_speed = speed;
-  }
-
-  /* Specify which units the speeds are given in */
-  msg.units = units;
-
-  /* Publishes message to set_motors topic */
-  set_motors_publisher.publish(msg);
-  ros::spinOnce();
-
-  return LIB_OK;
+    check_which_ok(which);
+
+    motors::set_motors msg;
+
+    if(!ros::ok())
+    {
+        return LIB_ERROR;
+    }
+
+    /* Set all the speeds (the booleans in the set_motors message determine
+     * which ones will be used) */
+    msg.fl_speed = speed_fl;
+    msg.fr_speed = speed_fr;
+    msg.bl_speed = speed_bl;
+    msg.br_speed = speed_br;
+
+    /* Tell the motors node which motors need to be updated */
+    msg.fl_set = msg.fr_set = msg.bl_set = msg.br_set = false;
+    if(which & (MOTOR_FL | MOTOR_FL_REV))
+    {
+        msg.fl_set = true;
+    }
+    if(which & (MOTOR_FR | MOTOR_FR_REV))
+    {
+        msg.fr_set = true;
+    }
+    if(which & (MOTOR_BL | MOTOR_BL_REV))
+    {
+        msg.bl_set = true;
+    }
+    if(which & (MOTOR_BR | MOTOR_BR_REV))
+    {
+        msg.br_set = true;
+    }
+
+    /* Specify which units the speeds are given in */
+    msg.units = units;
+
+    /* Publishes message to set_motors topic */
+    set_motors_publisher.publish(msg);
+    ros::spinOnce();
+
+    return LIB_OK;
 }
 
-/*!
- * \brief Query the current speeds of the motors
+/**
+ * Double-checks the which variable to make sure the front and back
+ * bits are not both set for a single motor.
+ *
+ * @param which The which motor specfication to check.
+ */
+void check_which_ok(int which)
+{
+    ROS_WARN_COND( ((which & MOTOR_FL) && (which & MOTOR_FL_REV)),
+        "FL Motor set in both directions!");
+    ROS_WARN_COND( ((which & MOTOR_FR) && (which & MOTOR_FR_REV)),
+        "FR Motor set in both directions!");
+    ROS_WARN_COND( ((which & MOTOR_BL) && (which & MOTOR_BL_REV)),
+        "BL Motor set in both directions!");
+    ROS_WARN_COND( ((which & MOTOR_BR) && (which & MOTOR_BR_REV)),
+        "BR Motor set in both directions!");
+}
+
+/**
+ * @brief Query the current speeds of the motors
  *
  * Sends a request to the query_motors service which will reply with the
  *  current speed of each motor.
  *
- * \param which A bitmask that will specify which motor speed should be
+ * @todo Change so we can get multiple motor speeds with one call
+ *
+ * @param which A bitmask that will specify which motor speed should be
  *  returned
- * \return The speed of the selected motor, or LIB_ERR if no motor selected
+ * @return The speed of the selected motor, or LIB_ERR if no motor selected
  */
-
-int motors_query(int which)
+float motors_query(int which)
 {
-  motors::query_motors srv;
-  if(query_motors_client.call(srv))
-	{
-    switch(which)
-		{
-      case MOTOR_FL:
-        return srv.response.fl_speed;
-      case MOTOR_FR:
-        return srv.response.fr_speed;
-      case MOTOR_BL:
-        return srv.response.bl_speed;
-      case MOTOR_BR:
-        return srv.response.br_speed;
-      default:
-        ROS_WARN("Bad WHICH in motors_query.");
+    motors::query_motors srv;
+    if(query_motors_client.call(srv))
+    {
+        switch(which)
+        {
+            case MOTOR_FL:
+                return srv.response.fl_speed;
+            case MOTOR_FR:
+                return srv.response.fr_speed;
+            case MOTOR_BL:
+                return srv.response.bl_speed;
+            case MOTOR_BR:
+                return srv.response.br_speed;
+            default:
+                ROS_WARN("Bad WHICH in motors_query.");
+                return LIB_ERROR;
+        }
+    }
+    else
+    {
+        ROS_ERROR("Failed to call service query_motors");
         return LIB_ERROR;
     }
-  }
-	else
-	{
-    ROS_ERROR("Failed to call service query_motors");
-    return LIB_ERROR;
-  }
-
-  return 0;
+
+    return 0;
 }
diff --git a/scout/libscout/src/libmotors.h b/scout/libscout/src/libmotors.h
index af6a1a5..8d0a987 100644
--- a/scout/libscout/src/libmotors.h
+++ b/scout/libscout/src/libmotors.h
@@ -62,14 +62,23 @@
 #define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
 #define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
 #define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
+
 #define MOTOR_PERCENT 'p'
 #define MOTOR_MMS 'm'
 #define MOTOR_CMS 'c'
 #define MOTOR_ABSOLUTE 'a'
+#define MOTOR_DEFAULT_UNIT MOTOR_PERCENT
 
 void libmotors_init();
-int motors_set(int speed, int which);
-int motors_query(int which);
+int motors_set(int which, float speed, char units=MOTOR_DEFAULT_UNIT);
+int motors_set_sides(float speed_l, float speed_r,
+                     char units=MOTOR_DEFAULT_UNIT);
+int motors_set_each(int which,
+                    float speed_fl, float speed_fr,
+                    float speed_bl, float speed_br,
+                    char units=MOTOR_DEFAULT_UNIT);
+void check_which_ok(int which);
+float motors_query(int which);
 
 #endif
 
diff --git a/scout/libscout/src/libscout.cpp b/scout/libscout/src/libscout.cpp
index 7eb611b..f8fa587 100644
--- a/scout/libscout/src/libscout.cpp
+++ b/scout/libscout/src/libscout.cpp
@@ -38,7 +38,7 @@
 #include "libscout.h"
 
 /* Global objects */
-ros::NodeHandle libscout_node;
+ros::NodeHandle node;
 
 /** \todo Decide how to call user behaviors
   * I'm thinking that there be a function call in main that calls the "main"
diff --git a/scout/libscout/src/tags b/scout/libscout/src/tags
index d887b1b..539d5b7 100644
--- a/scout/libscout/src/tags
+++ b/scout/libscout/src/tags
@@ -15,7 +15,7 @@ _LIBMOTORS_H_	libmotors.h	41
 _LIBSCOUT_H_	libscout.h	41
 init	libscout.cpp	/^int init(int modules){$/
 libmotors_init	libmotors.cpp	/^void libmotors_init(){$/
-libscout_node	constants.h	58
+node	constants.h	58
 motors_query	libmotors.cpp	/^int motors_query(char which){$/
 motors_set	libmotors.cpp	/^void motors_set(int speed, char which){$/
 motors_speed_set	libmotors.cpp	/^void motors_speed_set(int speed, char which){$/
diff --git a/scout/motors/manifest.xml b/scout/motors/manifest.xml
index c512137..aa18d32 100644
--- a/scout/motors/manifest.xml
+++ b/scout/motors/manifest.xml
@@ -3,13 +3,17 @@
      motors
      Module to drive the motors and provide all motor control functionality.
   </description>
-  <author>Ben</author>
+  <author>Ben Wasserman</author>
   <license>BSD</license>
   <review status="unreviewed" notes=""/>
   <url>https://www.roboticsclub.org/redmine/projects/colonyscout/wiki</url>
   <depend package="roscpp"/>
   <depend package="std_msgs"/>
 
+  <export>
+    <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmotors"/>
+  </export>
+
 </package>
 
 
diff --git a/scout/motors/msg/set_motors.msg b/scout/motors/msg/set_motors.msg
index aa1fe53..5371de7 100644
--- a/scout/motors/msg/set_motors.msg
+++ b/scout/motors/msg/set_motors.msg
@@ -1,7 +1,21 @@
 Header header
-int8 fl_speed
-int8 fr_speed
-int8 bl_speed
-int8 br_speed
+
+# Keeps track of which motors need to be changed
 int8 which
+
+# Set these to true if the motor should be updated
+bool fl_set
+bool fr_set
+bool bl_set
+bool br_set
+
+# The motor speeds, from -255 to 255 if absolute
+# -100 to 100 if percentage
+# Decimal values if in measured units
+float32 fl_speed
+float32 fr_speed
+float32 bl_speed
+float32 br_speed
+
+# Allows motor speeds to be specified mm/s, cm/s, percent, or absolute
 int8 units
diff --git a/scout/motors/src/motors.cpp b/scout/motors/src/motors.cpp
index e881258..7709c36 100644
--- a/scout/motors/src/motors.cpp
+++ b/scout/motors/src/motors.cpp
@@ -34,6 +34,7 @@
  **/
 
 #include "ros/ros.h"
+#include "motors_util.cpp"
 #include "motors.h"
 //#include "libscout/src/constants.h"
 #include <cstdlib>
@@ -45,15 +46,16 @@
  * @{
  **/
 
+
+
 /* Motor state variables
  * Speeds expressed as absolute speed out of max speed (0 - +-MAXSPEED)
  * Absolute speed is the speed written to the hardware to move the motors
  */
-/** @todo Fix types: static */
-int motor_fl_speed; /**< The current speed of the front left motor. */
-int motor_fr_speed; /**< The current speed of the front right motor. */
-int motor_bl_speed; /**< The current speed of the back left motor. */
-int motor_br_speed; /**< The current speed of the back right motor. */
+static int motor_fl_speed; /**< The current speed of the front left motor. */
+static int motor_fr_speed; /**< The current speed of the front right motor. */
+static int motor_bl_speed; /**< The current speed of the back left motor. */
+static int motor_br_speed; /**< The current speed of the back right motor. */
 
 /**
  * @brief Sets motor speed
@@ -65,43 +67,25 @@ int motor_br_speed; /**< The current speed of the back right motor. */
  */
 void motors_set(const motors::set_motors::ConstPtr& msg)
 {
-    /** @todo Edit to only set requested motors, not all */
-    int which = msg->which;
-    if(which & MOTOR_FL_REV)
-    {
-      motor_fl_speed = -1 * motors_rel_to_abs(msg->fl_speed, msg->units);
-    }
-    if(which & MOTOR_FR_REV)
-    {
-      motor_fr_speed = -1 * motors_rel_to_abs(msg->fr_speed, msg->units);
-    }
-    if(which & MOTOR_BL_REV)
-    {
-      motor_bl_speed = -1 * motors_rel_to_abs(msg->bl_speed, msg->units);
-    }
-    if(which & MOTOR_BR_REV)
-    {
-      motor_br_speed = -1 * motors_rel_to_abs(msg->br_speed, msg->units);
-    }
-    if(which & MOTOR_FL)
+    if(msg->fl_set)
     {
       motor_fl_speed = motors_rel_to_abs(msg->fl_speed, msg->units);
     }
-    if(which & MOTOR_FR)
+    if(msg->fr_set)
     {
       motor_fr_speed = motors_rel_to_abs(msg->fr_speed, msg->units);
     }
-    if(which & MOTOR_BL)
+    if(msg->bl_set)
     {
       motor_bl_speed = motors_rel_to_abs(msg->bl_speed, msg->units);
     }
-    if(which & MOTOR_BR)
+    if(msg->br_set)
     {
       motor_br_speed = motors_rel_to_abs(msg->br_speed, msg->units);
     }
 
-    /* Write speeds to hardware */
     /** @todo Add code to write speeds to hardware */
+    ROS_DEBUG("Motor speeds set");
 }
 
 /**
@@ -126,60 +110,6 @@ bool motors_query(motors::query_motors::Request &req,
 }
 
 /**
- * @brief Converts set speeds (of various units) to absolute speeds.
- *
- * @param speed The speed expressed in the desired units
- * @param units The units the desired speed is measured in
- * @return The absolute speed of the motor
- **/
-int motors_rel_to_abs(int rel_speed, int units)
-{
-    switch(units)
-    {
-      case MOTOR_ABSOLUTE:/* Speed given as absolute */
-        return rel_speed;
-      case MOTOR_PERCENT:/* Convert from percentage */
-        return rel_speed * MAXSPEED / 100;
-      case MOTOR_MMS:/* Convert from mm/s */
-        /** @todo Make math to do this conversion **/
-        return rel_speed;
-      case MOTOR_CMS:/* Convert from cm/s */
-        /** @todo Make math to do this conversion **/
-        return rel_speed;
-      default: /* The units aren't recognized */
-        /** @todo Decide on default case. Either percent or absolute. **/
-        return rel_speed;
-    }
-}
-
-/**
- * @brief Convert absolute speeds to speeds of various units.
- *
- * @param speed The speed expressed in absolute units.
- * @param units The units the desired speed is measured in.
- * @return The relative speed of the motor in desired units.
- **/
-int motors_abs_to_rel(int abs_speed, int units)
-{
-    switch(units)
-    {
-      case MOTOR_ABSOLUTE:/* Speed given as absolute */
-        return abs_speed;
-      case MOTOR_PERCENT:/* Convert from percentage */
-        return abs_speed * 100 / MAXSPEED;
-      case MOTOR_MMS:/* Convert from mm/s */
-        /** @todo Make math to do this conversion **/
-        return abs_speed;
-      case MOTOR_CMS:/* Convert from cm/s */
-        /** @todo Make math to do this conversion **/
-        return abs_speed;
-      default: /* The units aren't recognized */
-        /** @todo Decide on default case. Either percent or absolute. **/
-        return abs_speed;
-    }
-}
-
-/**
  * @brief Motors driver. This is a ROS node that controls motor speeds.
  *
  * This is the main function for the motors node. It is run when the node
@@ -196,12 +126,12 @@ int main(int argc, char **argv)
     ros::init(argc, argv, "motors_driver");
 
     /* Advertise that this serves the query_motors service */
-    ros::NodeHandle n;
-    ros::ServiceServer service = n.advertiseService("query_motors",
-                                                    motors_query);
+    ros::NodeHandle node;
+    ros::ServiceServer service = node.advertiseService("query_motors",
+                                                       motors_query);
 
     /* Subscribe to the set_motors topic */
-    ros::Subscriber sub0 = n.subscribe("set_motors", QUEUE_SIZE, motors_set);
+    ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set);
 
     /* Initialize hardware for motors */
     // Hardware init functions here
diff --git a/scout/motors/src/motors.h b/scout/motors/src/motors.h
index 24098ef..476cc16 100644
--- a/scout/motors/src/motors.h
+++ b/scout/motors/src/motors.h
@@ -40,26 +40,7 @@
 #include "motors/query_motors.h"
 #include "motors/set_motors.h"
 
-#define MAXSPEED 512
-#define MOTOR_ALL 0xF
-#define MOTOR_ALL_REV 0xF0
-#define MOTOR_NONE 0x0
-#define MOTOR_FL 0x8
-#define MOTOR_FR 0x4
-#define MOTOR_BL 0x2
-#define MOTOR_BR 0x1
-#define MOTOR_FL_REV 0x80
-#define MOTOR_FR_REV 0x40
-#define MOTOR_BL_REV 0x20
-#define MOTOR_BR_REV 0x10
-#define MOTOR_FRONT MOTOR_FL | MOTOR_FR
-#define MOTOR_BACK MOTOR_BR | MOTOR_BR
-#define MOTOR_LEFT MOTOR_FL | MOTOR_BL
-#define MOTOR_RIGHT MOTOR_FR | MOTOR_BR
-#define MOTOR_LEFT_REV MOTOR_FL_REV | MOTOR_BL_REV
-#define MOTOR_RIGHT_REV MOTOR_FR_REV | MOTOR_BR_REV
-#define MOTOR_LEFT_SPIN MOTOR_LEFT_REV | MOTOR_RIGHT
-#define MOTOR_RIGHT_SPIN MOTOR_LEFT | MOTOR_RIGHT_REV
+#define MAXSPEED 255
 #define MOTOR_PERCENT 'p'
 #define MOTOR_MMS 'm'
 #define MOTOR_CMS 'c'
