commit 560d231775789e9cd435c6d85515171a9375c811
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Sun Oct 14 16:41:43 2012 -0400

    Added forward/backward support to motors node

diff --git a/scout/motors/src/motors.cpp b/scout/motors/src/motors.cpp
index 9fa6d7a..aaa7bdb 100644
--- a/scout/motors/src/motors.cpp
+++ b/scout/motors/src/motors.cpp
@@ -31,11 +31,11 @@
  *
  * @author Colony Project, CMU Robotics Club
  * @author Ben Wasserman
+ * @author Tom Mullins
  */
 
 #include <ros/ros.h>
 #include <cstdlib>
-#include <fstream>
 #include "motors.h"
 
 using namespace std;
@@ -47,19 +47,114 @@ using namespace std;
  * @{
  */
 
-/* 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
+/**
+ * @brief Motor constructor
+ *
+ * Opens device file and reads initial PWM value
+ *
+ * @param filename The name of the PWM device file
  */
-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. */
+Motor::Motor(const char* pwmfile, const char* in1file, const char* in2file)
+  : fpwm(pwmfile), fin1(in1file), fin2(in2file)
+{
+  int pwm, in1, in2;
+
+  // read current values from device files
+  fpwm >> pwm;
+  fin1 >> in1;
+  fin2 >> in2;
+
+  // convert to our speed units
+  if (in1)
+  {
+    if (in2)
+    {
+      // short brake
+      speed = 0;
+    }
+    else
+    {
+      // CW
+      speed = pwm;
+    }
+  }
+  else
+  {
+    if (in2)
+    {
+      // CCW
+      speed = -pwm;
+    }
+    else
+    {
+      // off
+      speed = 0;
+    }
+  }
 
-static fstream motor_fl_file; /**< The device file for the front left motor. */
-static fstream motor_fr_file; /**< The device file for the front right motor. */
-static fstream motor_bl_file; /**< The device file for the back left motor. */
-static fstream motor_br_file; /**< The device file for the back right motor. */
+  // ensure we are in a consistent state by writing back to hardware
+  set_speed(speed);
+}
+
+/**
+ * @brief Returns current motor speed
+ *
+ * Note that this doesn't read from the hardware
+ */
+int Motor::get_speed()
+{
+  return speed;
+}
+
+/**
+ * @brief Sets motor speed
+ *
+ * This will set the member this->speed and write the new speed to the hardware
+ *
+ * @param new_speed The speed to set, between +/- MAXSPEED inclusive
+ */
+void Motor::set_speed(int new_speed)
+{
+  int pwm, in1, in2;
+
+  speed = new_speed;
+
+  // convert to hardware units
+  if (speed == 0)
+  {
+    /// @todo should this be off (00) or short brake (11)?
+    pwm = 0;
+    in1 = 0;
+    in2 = 0;
+  }
+  else if (speed > 0)
+  {
+    // CW
+    pwm = speed;
+    in1 = 1;
+    in2 = 0;
+  }
+  else
+  {
+    // CCW
+    pwm = -speed;
+    in1 = 0;
+    in2 = 1;
+  }
+
+  // write to hardware
+  fpwm << pwm;
+  fin1 << in1;
+  fin2 << in2;
+}
+
+/// @todo change these to the correct files
+/// @todo also, a problem: gpio can only be done as root!
+// Motor state variables
+static Motor motor_fl("/dev/pwm8", "/dev/null", "/dev/null");
+static Motor motor_fr("/dev/pwm9", "/dev/null", "/dev/null");
+static Motor motor_bl("/dev/pwm10", "/dev/null", "/dev/null");
+static Motor motor_br("/dev/pwm11", "/dev/null", "/dev/null");
 
 /**
  * @brief Sets motor speed
@@ -73,23 +168,19 @@ void motors_set(const motors::set_motors::ConstPtr& msg)
 {
     if(msg->fl_set)
     {
-      motor_fl_speed = msg->fl_speed;
-      motor_fl_file << motor_fl_speed;
+      motor_fl.set_speed(msg->fl_speed);
     }
     if(msg->fr_set)
     {
-      motor_fr_speed = msg->fr_speed;
-      motor_fr_file << motor_fr_speed;
+      motor_fr.set_speed(msg->fr_speed);
     }
     if(msg->bl_set)
     {
-      motor_bl_speed = msg->bl_speed;
-      motor_bl_file << motor_bl_speed;
+      motor_bl.set_speed(msg->bl_speed);
     }
     if(msg->br_set)
     {
-      motor_br_speed = msg->br_speed;
-      motor_br_file << motor_br_speed;
+      motor_br.set_speed(msg->br_speed);
     }
 
     ROS_DEBUG("Motor speeds set");
@@ -106,14 +197,10 @@ void motors_set(const motors::set_motors::ConstPtr& msg)
 bool motors_query(motors::query_motors::Request &req,
                   motors::query_motors::Response &res)
 {
-    /** @todo Are we checking hardware or just using the saved values?
-     *  Saved values sound fine to me (Alex). Pleas confirm.
-     *  I think saved values are ok; they're read in from hardware at the
-     *  beginning (Tom). */
-    res.fl_speed = motor_fl_speed;
-    res.fr_speed = motor_fr_speed;
-    res.bl_speed = motor_bl_speed;
-    res.br_speed = motor_br_speed;
+    res.fl_speed = motor_fl.get_speed();
+    res.fr_speed = motor_fr.get_speed();
+    res.bl_speed = motor_bl.get_speed();
+    res.br_speed = motor_br.get_speed();
 
     ROS_DEBUG("Motor speeds queried");
 
@@ -144,19 +231,6 @@ int main(int argc, char **argv)
     /* Subscribe to the set_motors topic */
     ros::Subscriber sub0 = node.subscribe("set_motors", QUEUE_SIZE, motors_set);
 
-    /* Open device files for PWM values */
-    /** @todo These are probably not the correct order */
-    motor_fl_file.open("/dev/pwm8");
-    motor_fr_file.open("/dev/pwm9");
-    motor_bl_file.open("/dev/pwm10");
-    motor_br_file.open("/dev/pwm11");
-
-    /* Get initial values, for motor queries */
-    motor_fl_file >> motor_fl_speed;
-    motor_fr_file >> motor_fr_speed;
-    motor_bl_file >> motor_bl_speed;
-    motor_br_file >> motor_br_speed;
-
     ROS_INFO("Motors node ready.");
     ros::spin();
 
