From 970d6a083f3cb04ebadde8769b3fd9f7735e6904 Mon Sep 17 00:00:00 2001
From: Jon Trulson <jtrulson@ics.com>
Date: Wed, 18 Mar 2015 12:20:19 -0600
Subject: [PATCH] grovemd: allow seperate directions for each DC motor

Signed-off-by: Jon Trulson <jtrulson@ics.com>
Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
---
 examples/c++/grovemd.cxx       |  4 ++--
 examples/javascript/grovemd.js |  6 ++++--
 src/grovemd/grovemd.cxx        |  5 +++--
 src/grovemd/grovemd.h          | 24 +++++++++++++++---------
 4 files changed, 24 insertions(+), 15 deletions(-)

diff --git a/examples/c++/grovemd.cxx b/examples/c++/grovemd.cxx
index 497b87de..a5c1015e 100644
--- a/examples/c++/grovemd.cxx
+++ b/examples/c++/grovemd.cxx
@@ -39,13 +39,13 @@ int main(int argc, char **argv)
 
   // set direction to CW and set speed to 50%
   cout << "Spin M1 and M2 at half speed for 3 seconds" << endl;
-  motors->setDirection(upm::GroveMD::DIR_CW);
+  motors->setMotorDirections(upm::GroveMD::DIR_CW, upm::GroveMD::DIR_CW);
   motors->setMotorSpeeds(127, 127);
   
   sleep(3);
   // counter clockwise
   cout << "Reversing M1 and M2 for 3 seconds" << endl;
-  motors->setDirection(upm::GroveMD::DIR_CCW);
+  motors->setMotorDirections(upm::GroveMD::DIR_CCW, upm::GroveMD::DIR_CCW);
   sleep(3);
 
   //! [Interesting]
diff --git a/examples/javascript/grovemd.js b/examples/javascript/grovemd.js
index 693f0b63..76109f4e 100644
--- a/examples/javascript/grovemd.js
+++ b/examples/javascript/grovemd.js
@@ -33,7 +33,8 @@ function start()
 	{
 		// set direction to CW and set speed to 50%
 		console.log("Spin M1 and M2 at half speed for 3 seconds");
-		my_MotorDriver_obj.setDirection(groveMotorDriver_lib.GroveMD.DIR_CW);
+		my_MotorDriver_obj.setMotorDirections(groveMotorDriver_lib.GroveMD.DIR_CW,
+                                                     groveMotorDriver_lib.GroveMD.DIR_CW);
 		my_MotorDriver_obj.setMotorSpeeds(127, 127);
 	}
 }
@@ -44,7 +45,8 @@ function reverse()
 	{
 		// counter clockwise
 		console.log("Reversing M1 and M2 for 3 seconds");
-		my_MotorDriver_obj.setDirection(groveMotorDriver_lib.GroveMD.DIR_CCW);	
+		my_MotorDriver_obj.setMotorDirections(groveMotorDriver_lib.GroveMD.DIR_CCW,
+                                                     groveMotorDriver_lib.GroveMD.DIR_CCW);	
 	}
 }
 
diff --git a/src/grovemd/grovemd.cxx b/src/grovemd/grovemd.cxx
index 2fa59598..2a965dd7 100644
--- a/src/grovemd/grovemd.cxx
+++ b/src/grovemd/grovemd.cxx
@@ -108,12 +108,13 @@ bool GroveMD::setPWMFrequencyPrescale(uint8_t freq)
   return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP);
 }
 
-bool GroveMD::setDirection(DIRECTION_T dir)
+bool GroveMD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB)
 {
+  uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
   return writePacket(SET_DIRECTION, dir, GROVEMD_NOOP);
 }
 
-bool GroveMD::enableStepper(DIRECTION_T dir, uint8_t speed)
+bool GroveMD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed)
 {
   return writePacket(STEPPER_ENABLE, dir, speed);
 }
diff --git a/src/grovemd/grovemd.h b/src/grovemd/grovemd.h
index 5d6f887d..bc934eb0 100644
--- a/src/grovemd/grovemd.h
+++ b/src/grovemd/grovemd.h
@@ -79,10 +79,15 @@ namespace upm {
                    STEPPER_NUM_STEPS   = 0x1c
     } REG_T;
 
-    // legal directions
-    typedef enum { DIR_CCW    = 0x0a,
-                   DIR_CW     = 0x05
-    } DIRECTION_T;
+    // legal directions for the stepper
+    typedef enum { STEP_DIR_CCW    = 0x0a,
+                   STEP_DIR_CW     = 0x05
+    } STEP_DIRECTION_T;
+    
+    // legal directions for individual DC motors
+    typedef enum { DIR_CCW    = 0x02,
+                   DIR_CW     = 0x01
+    } DC_DIRECTION_T;
     
     /**
      * grovemd constructor
@@ -130,22 +135,23 @@ namespace upm {
     bool setPWMFrequencyPrescale(uint8_t freq=0x03);
 
     /**
-     * For controlling DC motors, set the direction
+     * For controlling DC motors, set the directions of motors A & B
      *
-     * @param dir direction, CW or CCW
+     * @param dirA direction for motor A, DIR_CW or DIR_CCW
+     * @param dirB direction for motor B, DIR_CW or DIR_CCW
      * @return true if command successful
      */
-    bool setDirection(DIRECTION_T dir);
+    bool setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB);
 
     /**
      * For controlling a stepper motor, set a direction, speed and
      * then enable.
      *
-     * @param dir direction, CW or CCW
+     * @param dir direction, STEP_DIR_CW or STEP_DIR_CCW
      * @param speed motor speed. Valid range is 1-255, higher is slower.
      * @return true if command successful
      */
-    bool enableStepper(DIRECTION_T dir, uint8_t speed);
+    bool enableStepper(STEP_DIRECTION_T dir, uint8_t speed);
 
     /**
      * For controlling a stepper motor, stop the stepper motor.