md: C implementation; C++ wraps C

Signed-off-by: Jon Trulson <jtrulson@ics.com>

examples/c++/md-stepper.cxx

@@ -1,6 +1,6 @@
 /*
  * Author: Jon Trulson <jtrulson@ics.com>
- * Copyright (c) 2015 Intel Corporation.
+ * Copyright (c) 2015-2016 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -34,8 +34,7 @@ int main(int argc, char **argv)
   //! [Interesting]
   // Instantiate an I2C Motor Driver on I2C bus 0
 
-  upm::MD *motors = new upm::MD(MD_I2C_BUS, 
+  upm::MD *motors = new upm::MD(MD_I2C_BUS, MD_DEFAULT_I2C_ADDR);
-                                          MD_DEFAULT_I2C_ADDR);
 
   // This example demonstrates using the MD to drive a stepper motor
   
@@ -46,13 +45,13 @@ int main(int argc, char **argv)
   motors->setStepperSteps(100);
 
   // let it go - clockwise rotation, 10 RPM speed
-  motors->enableStepper(upm::MD::STEP_DIR_CW, 10);
+  motors->enableStepper(MD_STEP_DIR_CW, 10);
 
   sleep(3);
 
   // Now do it backwards...
   motors->setStepperSteps(100);
-  motors->enableStepper(upm::MD::STEP_DIR_CCW, 10);
+  motors->enableStepper(MD_STEP_DIR_CCW, 10);
 
   // now disable
   motors->disableStepper();

examples/c++/md.cxx

@@ -1,6 +1,6 @@
 /*
  * Author: Jon Trulson <jtrulson@ics.com>
- * Copyright (c) 2014 Intel Corporation.
+ * Copyright (c) 2014-2016 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -34,18 +34,17 @@ int main(int argc, char **argv)
   //! [Interesting]
   // Instantiate an I2C Motor Driver on I2C bus 0
 
-  upm::MD *motors = new upm::MD(MD_I2C_BUS, 
+  upm::MD *motors = new upm::MD(MD_I2C_BUS, MD_DEFAULT_I2C_ADDR);
-                                          MD_DEFAULT_I2C_ADDR);
 
   // set direction to CW and set speed to 50%
   cout << "Spin M1 and M2 at half speed for 3 seconds" << endl;
-  motors->setMotorDirections(upm::MD::DIR_CW, upm::MD::DIR_CW);
+  motors->setMotorDirections(MD_DIR_CW, MD_DIR_CW);
   motors->setMotorSpeeds(127, 127);
   
   sleep(3);
   // counter clockwise
   cout << "Reversing M1 and M2 for 3 seconds" << endl;
-  motors->setMotorDirections(upm::MD::DIR_CCW, upm::MD::DIR_CCW);
+  motors->setMotorDirections(MD_DIR_CCW, MD_DIR_CCW);
   sleep(3);
 
   //! [Interesting]

examples/c/CMakeLists.txt

@@ -127,9 +127,11 @@ add_example (jhd1313m1)
 add_example (lm35)
 add_example (rotaryencoder)
 add_example (rpr220)
+add_example (md)
 
 # Custom examples
 add_custom_example (nmea_gps_i2c-example-c nmea_gps_i2c.c nmea_gps)
 add_custom_example (lcm1602-i2c-example-c lcm1602-i2c.c lcm1602)
 add_custom_example (lcm1602-parallel-example-c lcm1602-parallel.c lcm1602)
 add_custom_example (rpr220-intr-example-c rpr220-intr.c rpr220)
+add_custom_example (md-stepper-example-c md-stepper.c md)

examples/c/md-stepper.c

@@ -0,0 +1,69 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+
+#include <md.h>
+#include <upm_utilities.h>
+
+int main(int argc, char **argv)
+{
+  //! [Interesting]
+  // Instantiate an I2C Motor Driver on I2C bus 0
+
+  md_context stepper = md_init(MD_I2C_BUS, MD_DEFAULT_I2C_ADDR);
+
+  if (!stepper)
+  {
+      printf("md_init() failed\n");
+      return 1;
+  }
+
+  // This example demonstrates using the MD to drive a stepper motor
+
+  // configure it, for this example, we'll assume 200 steps per rev
+  md_config_stepper(stepper, 200, MD_STEP_MODE1);
+
+  // set for half a rotation
+  md_set_stepper_steps(stepper, 100);
+
+  // let it go - clockwise rotation, 10 RPM speed
+  md_enable_stepper(stepper, MD_STEP_DIR_CW, 10);
+
+  upm_delay(3);
+
+  // Now do it backwards...
+  md_set_stepper_steps(stepper, 100);
+  md_enable_stepper(stepper, MD_STEP_DIR_CCW, 10);
+
+  // now disable
+  md_disable_stepper(stepper);
+
+  printf("Exiting...\n");
+
+  md_close(stepper);
+  //! [Interesting]
+  return 0;
+}

examples/c/md.c

@@ -0,0 +1,64 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <stdio.h>
+
+#include <md.h>
+#include <upm_utilities.h>
+
+
+int main(int argc, char **argv)
+{
+//! [Interesting]
+  // Instantiate an I2C Motor Driver on I2C bus 0
+
+  md_context motors = md_init(MD_I2C_BUS, MD_DEFAULT_I2C_ADDR);
+
+  if (!motors)
+  {
+      printf("md_init() failed\n");
+      return 1;
+  }
+
+  // set direction to CW and set speed to 50%
+  printf("Spin M1 and M2 at half speed for 3 seconds\n");
+  md_set_motor_directions(motors, MD_DIR_CW, MD_DIR_CW);
+  md_set_motor_speeds(motors, 127, 127);
+
+  upm_delay(3);
+  // counter clockwise
+  printf("Reversing M1 and M2 for 3 seconds\n");
+  md_set_motor_directions(motors, MD_DIR_CCW, MD_DIR_CCW);
+  upm_delay(3);
+
+  printf("Stopping motors\n");
+  md_set_motor_speeds(motors, 0, 0);
+
+  printf("Exiting...\n");
+
+  md_close(motors);
+//! [Interesting]
+  return 0;
+}

examples/java/MDSample.java

@@ -33,15 +33,15 @@ public class MDSample {
 
 		// set direction to clockwise (CW) and set speed to 50%
 		System.out.println("Spin M1 and M2 at half speed for 3 seconds");
-		motors.setMotorDirections(upm_md.MD.DC_DIRECTION_T.DIR_CW,
+		motors.setMotorDirections(upm_md.MD_DC_DIRECTION_T.MD_DIR_CW,
-				upm_md.MD.DC_DIRECTION_T.DIR_CW);
+                                          upm_md.MD_DC_DIRECTION_T.MD_DIR_CW);
 		motors.setMotorSpeeds(speed50, speed50);
 		Thread.sleep(3000);
 
 		// counter clockwise (CCW)
 		System.out.println("Reversing M1 and M2 for 3 seconds");
-		motors.setMotorDirections(upm_md.MD.DC_DIRECTION_T.DIR_CCW,
+		motors.setMotorDirections(upm_md.MD_DC_DIRECTION_T.MD_DIR_CCW,
-				upm_md.MD.DC_DIRECTION_T.DIR_CCW);
+                                          upm_md.MD_DC_DIRECTION_T.MD_DIR_CCW);
 		Thread.sleep(3000);
 
 		// stop motors

examples/python/md-stepper.py

@@ -40,13 +40,13 @@ def main():
     myMotorDriver.setStepperSteps(100)
 
     # let it go - clockwise rotation, 10 RPM speed
-    myMotorDriver.enableStepper(upmmd.MD.STEP_DIR_CW, 10)
+    myMotorDriver.enableStepper(upmmd.MD_STEP_DIR_CW, 10)
 
     time.sleep(3)
 
     # Now do it backwards...
     myMotorDriver.setStepperSteps(100)
-    myMotorDriver.enableStepper(upmmd.MD.STEP_DIR_CCW, 10)
+    myMotorDriver.enableStepper(upmmd.MD_STEP_DIR_CCW, 10)
 
     # now disable
     myMotorDriver.disableStepper()

examples/python/md.py

@@ -34,14 +34,14 @@ def main():
 
     # set direction to CW and set speed to 50%
     print("Spin M1 and M2 at half speed for 3 seconds")
-    myMotorDriver.setMotorDirections(upmmd.MD.DIR_CW, upmmd.MD.DIR_CW)
+    myMotorDriver.setMotorDirections(upmmd.MD_DIR_CW, upmmd.MD_DIR_CW)
     myMotorDriver.setMotorSpeeds(127, 127)
 
     time.sleep(3)
     # counter clockwise
     print("Reversing M1 and M2 for 3 seconds")
-    myMotorDriver.setMotorDirections(upmmd.MD.DIR_CCW,
+    myMotorDriver.setMotorDirections(upmmd.MD_DIR_CCW,
-    upmmd.MD.DIR_CCW)
+                                     upmmd.MD_DIR_CCW)
     time.sleep(3)
 
     print("Stopping motors")

src/md/CMakeLists.txt

@@ -1,5 +1,9 @@
 upm_mixed_module_init (NAME md
-    DESCRIPTION "UPM MD Module"
+    DESCRIPTION "MD - Grove Motor Driver"
+    C_HDR md.h
+    C_SRC md.c
     CPP_HDR md.hpp
     CPP_SRC md.cxx
+#    FTI_SRC md_fti.c
+    CPP_WRAPS_C
     REQUIRES mraa)

src/md/javaupm_md.i

@@ -5,6 +5,7 @@
     #include "md.hpp"
 %}
 
+%include "md.h"
 %include "md.hpp"
 
 %pragma(java) jniclasscode=%{

src/md/jsupm_md.i

@@ -1,8 +1,9 @@
 %module jsupm_md
 %include "../upm.i"
 
+%include "md.h"
+%include "md.hpp"
 %{
     #include "md.hpp"
 %}
 
-%include "md.hpp"

src/md/md.c

@@ -0,0 +1,265 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <unistd.h>
+#include <assert.h>
+#include <string.h>
+
+#include <upm_utilities.h>
+
+#include "md.h"
+
+// static (private) functions
+static void md_stepper_step(const md_context dev)
+{
+    assert(dev != NULL);
+
+    int step = dev->currentStep % 4;
+
+    switch (step)
+    {
+    case 0:
+        md_write_packet(dev, MD_REG_SET_DIRECTION, 0b0101, MD_NOOP);
+        break;
+    case 1:
+        md_write_packet(dev, MD_REG_SET_DIRECTION, 0b0110, MD_NOOP);
+        break;
+    case 2:
+        md_write_packet(dev, MD_REG_SET_DIRECTION, 0b1010, MD_NOOP);
+        break;
+    case 3:
+        md_write_packet(dev, MD_REG_SET_DIRECTION, 0b1001, MD_NOOP);
+        break;
+    default:
+        // can't happen in this universe
+        break;
+    }
+}
+
+md_context md_init(int bus, uint8_t address)
+{
+    md_context dev =
+        (md_context)malloc(sizeof(struct _md_context));
+
+    if (!dev)
+        return NULL;
+
+    memset((void *)dev, 0, sizeof(struct _md_context));
+    dev->i2c = NULL;
+    dev->initialized = false;
+
+    // make sure MRAA is initialized
+    int mraa_rv;
+    if ((mraa_rv = mraa_init()) != MRAA_SUCCESS)
+    {
+        printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv);
+        md_close(dev);
+        return NULL;
+    }
+
+    // MRAA contexts...
+    if ( !(dev->i2c = mraa_i2c_init(bus)) )
+    {
+        printf("%s: mraa_gpio_init() failed\n",
+               __FUNCTION__);
+        md_close(dev);
+        return NULL;
+    }
+
+    // this board *requires* 100Khz i2c bus only
+    if (mraa_i2c_frequency(dev->i2c, MRAA_I2C_STD))
+    {
+        printf("%s: mraa_i2c_frequency(MRAA_I2C_STD) failed\n", __FUNCTION__);
+        md_close(dev);
+        return NULL;
+    }
+
+    if (mraa_i2c_address(dev->i2c, address))
+    {
+        printf("%s: mraa_i2c_frequency(MRAA_I2C_STD) failed\n", __FUNCTION__);
+        md_close(dev);
+        return NULL;
+    }
+
+    // default to mode1 stepper operation, 200 steps per rev.
+    md_config_stepper(dev, 200, MD_STEP_MODE1);
+
+    dev->initialized = true;
+
+    return dev;
+}
+
+void md_close(md_context dev)
+{
+    assert(dev != NULL);
+
+    if (dev->initialized)
+    {
+        md_set_motor_speeds(dev, 0, 0);
+        md_write_packet(dev, MD_REG_SET_DIRECTION, 0, MD_NOOP);
+    }
+
+    if (dev->i2c)
+        mraa_i2c_stop(dev->i2c);
+
+    free(dev);
+}
+
+bool md_write_packet(const md_context dev, MD_REG_T reg,
+                     uint8_t data1, uint8_t data2)
+{
+    assert(dev != NULL);
+
+    uint8_t buf[3];
+
+    buf[0] = reg;
+    buf[1] = data1;
+    buf[2] = data2;
+
+    if (mraa_i2c_write(dev->i2c, buf, 3))
+    {
+        printf("%s: mraa_i2c_write() failed\n", __FUNCTION__);
+        return false;
+    }
+
+    // This sleep appears to be required.  Without it, writes randomly
+    // fail (no ACK received).  This happens most often on the SET_SPEED
+    // packet.  I am guessing that there is a timing problem and/or bug
+    // in the motor driver's firmware.
+
+    upm_delay_us(100);
+
+    return true;
+}
+
+bool md_set_motor_speeds(const md_context dev, uint8_t speedA, uint8_t speedB)
+{
+    assert(dev != NULL);
+
+    return md_write_packet(dev, MD_REG_SET_SPEED, speedA, speedB);
+}
+
+bool md_set_pwm_frequency_prescale(const md_context dev, uint8_t freq)
+{
+    assert(dev != NULL);
+
+    return md_write_packet(dev, MD_REG_SET_PWM_FREQ, freq, MD_NOOP);
+}
+
+bool md_set_motor_directions(const md_context dev, MD_DC_DIRECTION_T dirA,
+                             MD_DC_DIRECTION_T dirB)
+{
+    assert(dev != NULL);
+
+    uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
+    return md_write_packet(dev, MD_REG_SET_DIRECTION, dir, MD_NOOP);
+}
+
+bool md_enable_stepper(const md_context dev, MD_STEP_DIRECTION_T dir,
+                       uint8_t speed)
+{
+    assert(dev != NULL);
+
+    // If mode 2, send the command and return immediately
+    if (dev->stepMode == MD_STEP_MODE2)
+        return md_write_packet(dev, MD_REG_STEPPER_ENABLE, dir, speed);
+
+    // otherwise, mode 1, setup the basics and start stepping.
+
+    dev->stepDelay = 60 * 1000 / dev->stepsPerRev / speed;
+    dev->stepDirection = ((dir == MD_STEP_DIR_CW) ? 1 : -1);
+
+    // seeed says speed should always be 255,255 for stepper operation
+    md_set_motor_speeds(dev, 255, 255);
+
+    while (dev->totalSteps > 0)
+    {
+        upm_delay_ms(dev->stepDelay);
+
+        dev->currentStep += dev->stepDirection;
+
+        if (dev->stepDirection == 1)
+        {
+            if (dev->currentStep >= dev->stepsPerRev)
+                dev->currentStep = 0;
+        }
+        else
+        {
+            if (dev->currentStep <= 0)
+                dev->currentStep = dev->stepsPerRev;
+        }
+
+        dev->totalSteps--;
+        md_stepper_step(dev);
+    }
+
+    // and... we're done
+    return true;
+}
+
+bool md_disable_stepper(const md_context dev)
+{
+    assert(dev != NULL);
+
+    if (dev->stepMode == MD_STEP_MODE2)
+        return md_write_packet(dev, MD_REG_STEPPER_DISABLE, MD_NOOP, MD_NOOP);
+
+    // else, mode 1
+    md_write_packet(dev, MD_REG_SET_DIRECTION, 0, MD_NOOP);
+    return md_set_motor_speeds(dev, 0, 0);
+}
+
+bool md_set_stepper_steps(const md_context dev, unsigned int steps)
+{
+    assert(dev != NULL);
+
+    if (dev->stepMode == MD_STEP_MODE2)
+    {
+        if (steps == 0)
+        {
+            // invalid
+            printf("%s: invalid number of steps. "
+                   "Valid values are between 1 and 255. \n", __FUNCTION__);
+            return false;
+        }
+        return md_write_packet(dev, MD_REG_STEPPER_NUM_STEPS, steps, MD_NOOP);
+    }
+
+    // for mode one, just store it for future use by enableStepper()
+    dev->totalSteps = steps;
+    return true;
+}
+
+void md_config_stepper(const md_context dev, unsigned int stepsPerRev,
+                       MD_STEP_MODE_T mode)
+{
+    assert(dev != NULL);
+
+    dev->stepsPerRev = stepsPerRev;
+    dev->stepMode = mode;
+    dev->currentStep = 0;
+    dev->stepDelay = 0;
+    dev->stepDirection = 1;
+    dev->totalSteps = 0;
+}

src/md/md.cxx

@@ -1,6 +1,6 @@
 /*
  * Author: Jon Trulson <jtrulson@ics.com>
- * Copyright (c) 2014 Intel Corporation.
+ * Copyright (c) 2014-2016 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -34,211 +34,71 @@ using namespace std;
 
 
 MD::MD(int bus, uint8_t address) :
-  m_i2c(bus)
+    m_md(md_init(bus, address))
 {
-  m_addr = address;
+    if (!m_md)
-
-  // this board *requires* 100Khz i2c bus only
-  mraa::Result rv;
-  if ( (rv = m_i2c.frequency(mraa::I2C_STD)) != mraa::SUCCESS )
-    {
-      throw std::invalid_argument(std::string(__FUNCTION__) +
-                                  ": I2c.frequency(I2C_STD) failed");
-      return;
-    }
-
-  if (m_i2c.address(m_addr))
-    {
         throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": I2c.address() failed");
+                                 ": md_init() failed");
-      return;
-    }
-
-  initClock();
-  // default to mode1 stepper operation, 200 steps per rev.
-  configStepper(200, STEP_MODE1);
 }
 
 MD::~MD()
 {
-  setMotorSpeeds(0, 0);
+    md_close(m_md);
-  writePacket(SET_DIRECTION, 0, MD_NOOP);
 }
 
-bool MD::writePacket(REG_T reg, uint8_t data1, uint8_t data2)
+bool MD::writePacket(MD_REG_T reg, uint8_t data1, uint8_t data2)
 {
-  uint8_t buf[3];
+    if (!md_write_packet(m_md, reg, data1, data2))
-
-  buf[0] = reg;
-  buf[1] = data1;
-  buf[2] = data2;
-
-  if ( m_i2c.write(buf, 3) != mraa::SUCCESS )
-    {
         throw std::runtime_error(std::string(__FUNCTION__) +
-                               ": I2c.write() failed");
+                                 ": md_write_packet() failed");
-      return false;
-    }
-
-  // This sleep appears to be required.  Without it, writes randomly
-  // fail (no ACK received).  This happens most often on the SET_SPEED
-  // packet.  I am guessing that there is a timing problem and/or bug
-  // in the motor driver's firmware.
-
-  usleep(100);
 
     return true;
 }
 
 bool MD::setMotorSpeeds(uint8_t speedA, uint8_t speedB)
 {
-  return writePacket(SET_SPEED, speedA, speedB);
+    if (!md_set_motor_speeds(m_md, speedA, speedB))
+        throw std::runtime_error(std::string(__FUNCTION__) +
+                                 ": md_set_motor_speeds() failed");
+
+    return true;
 }
 
 bool MD::setPWMFrequencyPrescale(uint8_t freq)
 {
-  return writePacket(SET_PWM_FREQ, freq, MD_NOOP);
+    if (!md_set_pwm_frequency_prescale(m_md, freq))
-}
+        throw std::runtime_error(std::string(__FUNCTION__) +
+                                 ": md_set_pwm_frequency_prescale() failed");
 
-bool MD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB)
-{
-  uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
-  return writePacket(SET_DIRECTION, dir, MD_NOOP);
-}
-
-bool MD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed)
-{
-  // If mode 2, send the command and return immediately
-  if (m_stepMode == STEP_MODE2)
-    return writePacket(STEPPER_ENABLE, dir, speed);
-
-  // otherwise, mode 1, setup the basics and start stepping.
-
-  m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
-  m_stepDirection = ((dir == STEP_DIR_CW) ? 1 : -1);
-
-  // seeed says speed should always be 255,255 for stepper operation
-  setMotorSpeeds(255, 255);
-
-  while (m_totalSteps > 0)
-    {
-      if (getMillis() >= m_stepDelay)
-        {
-          // reset the clock
-          initClock();
-
-          m_currentStep += m_stepDirection;
-
-          if (m_stepDirection == 1)
-            {
-              if (m_currentStep >= m_stepsPerRev)
-                m_currentStep = 0;
-            }
-          else
-            {
-              if (m_currentStep <= 0)
-                m_currentStep = m_stepsPerRev;
-            }
-
-          m_totalSteps--;
-          stepperStep();
-        }
-    }
-
-  // and... we're done
     return true;
 }
 
+bool MD::setMotorDirections(MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB)
+{
+    if (!md_set_motor_directions(m_md, dirA, dirB))
+        throw std::runtime_error(std::string(__FUNCTION__) +
+                                 ": md_set_motor_directions() failed");
+
+    return true;
+}
+
+bool MD::enableStepper(MD_STEP_DIRECTION_T dir, uint8_t speed)
+{
+    return md_enable_stepper(m_md, dir, speed);
+}
+
 bool MD::disableStepper()
 {
-  if (m_stepMode == STEP_MODE2)
+    return md_disable_stepper(m_md);
-    return writePacket(STEPPER_DISABLE, MD_NOOP, MD_NOOP);
-
-  // else, mode 1
-  writePacket(SET_DIRECTION, 0, MD_NOOP);
-  return setMotorSpeeds(0, 0);
 }
 
 bool MD::setStepperSteps(unsigned int steps)
 {
-  if (m_stepMode == STEP_MODE2)
+    return md_set_stepper_steps(m_md, steps);
-    {
-      if (steps == 0)
-        {
-          // invalid
-          throw std::out_of_range(std::string(__FUNCTION__) +
-                                  ": invalid number of steps.  " +
-                                  "Valid values are between 1 and 255.");
-          return false;
-        }
-      return writePacket(STEPPER_NUM_STEPS, steps, MD_NOOP);
-    }
-
-  // for mode one, just store it for future use by enableStepper()
-  m_totalSteps = steps;
-  return true;
 }
 
-void MD::initClock()
+void MD::configStepper(unsigned int stepsPerRev, MD_STEP_MODE_T mode)
 {
-  gettimeofday(&m_startTime, NULL);
+    md_config_stepper(m_md, stepsPerRev, mode);
 }
 
-uint32_t MD::getMillis()
-{
-  struct timeval elapsed, now;
-  uint32_t elapse;
-
-  // get current time
-  gettimeofday(&now, NULL);
-
-  // compute the delta since m_startTime
-  if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
-    {
-      elapsed.tv_usec += 1000000;
-      elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
-    }
-  else
-    {
-      elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
-    }
-
-  elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
-
-  // never return 0
-  if (elapse == 0)
-    elapse = 1;
-
-  return elapse;
-}
-
-void MD::configStepper(unsigned int stepsPerRev, STEP_MODE_T mode)
-{
-  m_stepsPerRev = stepsPerRev;
-  m_stepMode = mode;
-  m_currentStep = 0;
-  m_stepDelay = 0;
-  m_stepDirection = 1;
-  m_totalSteps = 0;
-}
-
-void MD::stepperStep()
-{
-  int step = m_currentStep % 4;
-
-  switch (step)
-    {
-    case 0:
-      writePacket(SET_DIRECTION, 0b0101, MD_NOOP);
-      break;
-    case 1:
-      writePacket(SET_DIRECTION, 0b0110, MD_NOOP);
-      break;
-    case 2:
-      writePacket(SET_DIRECTION, 0b1010, MD_NOOP);
-      break;
-    case 3:
-      writePacket(SET_DIRECTION, 0b1001, MD_NOOP);
-      break;
-    }
-}

src/md/md.h

@@ -0,0 +1,220 @@
+/*
+ * Author: Jon Trulson <jtrulson@ics.com>
+ * Copyright (c) 2016 Intel Corporation.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sublicense, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
+ * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
+ * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
+ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <upm.h>
+#include <mraa/i2c.h>
+
+#define MD_I2C_BUS 0
+#define MD_DEFAULT_I2C_ADDR 0x0f
+
+// This is a NOOP value used to pad packets
+#define MD_NOOP 0x01
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    /**
+     * @file md.h
+     * @library md
+     * @brief C API for the md driver
+     *
+     * @include md.c
+     */
+
+    // MD registers
+    typedef enum {
+        MD_REG_SET_SPEED           = 0x82,
+        MD_REG_SET_PWM_FREQ        = 0x84,
+        MD_REG_SET_DIRECTION       = 0xaa,
+        MD_REG_SET_MOTOR_A         = 0xa1, // not documented
+        MD_REG_SET_MOTOR_B         = 0xa5, // not documented
+        MD_REG_STEPPER_ENABLE      = 0x1a,
+        MD_REG_STEPPER_DISABLE     = 0x1b,
+        MD_REG_STEPPER_NUM_STEPS   = 0x1c
+    } MD_REG_T;
+
+    // legal directions for the stepper
+    typedef enum {
+        MD_STEP_DIR_CCW    = 0x01,
+        MD_STEP_DIR_CW     = 0x00
+    } MD_STEP_DIRECTION_T;
+
+    // legal directions for individual DC motors
+    typedef enum {
+        MD_DIR_CCW    = 0x02,
+        MD_DIR_CW     = 0x01
+    } MD_DC_DIRECTION_T;
+
+    // stepper modes
+    typedef enum {
+        MD_STEP_MODE1 = 0x00,
+        MD_STEP_MODE2 = 0x01
+    } MD_STEP_MODE_T;
+
+    /**
+     * Device context
+     */
+    typedef struct _md_context {
+        mraa_i2c_context i2c;
+
+        // steps per revolution
+        int stepsPerRev;
+        int currentStep;
+        uint32_t stepDelay;
+        uint32_t totalSteps;
+        MD_STEP_MODE_T stepMode;
+
+        // step direction: - 1 = forward, -1 = backward
+        int stepDirection;
+
+        // initialized?
+        bool initialized;
+
+    } *md_context;
+
+    /**
+     * MD initializer.
+     *
+     * @param bus I2C bus to use
+     * @param address I2C address to use
+     * @return Device context.
+     */
+    md_context md_init(int bus, uint8_t address);
+
+    /**
+     * MD close.
+     *
+     * @param dev Device context.
+     */
+    void md_close(md_context dev);
+
+    /**
+     * Composes and writes a 3-byte packet to the controller
+     *
+     * @param dev Device context.
+     * @param reg Register location
+     * @param data1 First byte of data
+     * @param data2 Second byte of data
+     * @return True if successful
+     */
+    bool md_write_packet(const md_context dev, MD_REG_T reg, uint8_t data1,
+                         uint8_t data2);
+
+    /**
+     * To control DC motors, sets the speed of motors A & B.
+     * Valid values are 0-255.
+     *
+     * @param dev Device context.
+     * @param speedA Speed of motor A
+     * @param speedB Speed of motor B
+     * @return True if successful
+     */
+    bool md_set_motor_speeds(const md_context dev, uint8_t speedA,
+                             uint8_t speedB);
+
+    /**
+     * To control DC motors, sets the PWM frequency prescale
+     * factor. Note: this register is not ducumented other than to say
+     * the default value is 0x03. Presumably, this is the timer
+     * prescale factor used on the ATMega MCU timer driving the PWM.
+     *
+     * @param dev Device context.
+     * @param freq PWM prescale frequency; default should be 0x03
+     * @return True if successful
+     */
+    bool md_set_pwm_frequency_prescale(const md_context dev, uint8_t freq);
+
+    /**
+     * To control DC motors, sets the directions of motors A & B
+     *
+     * @param dev Device context.
+     * @param dirA Direction for motor A, MD_DIR_CW or MD_DIR_CCW
+     * @param dirB Direction for motor B, MD_DIR_CW or MD_DIR_CCW
+     * @return True if successful
+     */
+    bool md_set_motor_directions(const md_context dev, MD_DC_DIRECTION_T dirA,
+                                 MD_DC_DIRECTION_T dirB);
+
+    /**
+     * To control a stepper motor, sets its direction and speed, and
+     * then starts operation.  For Mode2, this method will return
+     * immediately.  For Mode1 (the default) this method returns when
+     * the number of steps specified by md_set_stepper_steps() has
+     * completed.
+     *
+     * @param dev Device context.
+     * @param dir Direction, MD_STEP_DIR_CW or MD_STEP_DIR_CCW
+     * @param speed Motor speed. Valid range is 1-255. For Mode 1
+     * (default), this specifies the speed in RPM's.  For Mode 2,
+     * speed is multiplied by 4ms by the board, so higher numbers
+     * will mean a slower speed.
+     * @return True if successful
+     */
+    bool md_enable_stepper(const md_context dev, MD_STEP_DIRECTION_T dir,
+                           uint8_t speed);
+
+    /**
+     * To control a stepper motor, stops the stepper motor.
+     *
+     * @param dev Device context.
+     * @return True if successful
+     */
+    bool md_disable_stepper(const md_context dev);
+
+    /**
+     * To control a stepper motor, specifies the number of steps to
+     * execute. For Mode2, valid values are between 1-255, 255 means
+     * continuous rotation.
+     *
+     * For Mode1 (the default) steps can be any positive integer.
+     *
+     * @param dev Device context.
+     * @param steps Number of steps to execute. 255 (only in Mode2)
+     * means continuous rotation.
+     * @return True if successful
+     */
+    bool md_set_stepper_steps(const md_context dev, unsigned int steps);
+
+    /**
+     * Configure the initial Stepper parameters.  This should be
+     * called before any other stepper method.
+     *
+     * @param dev Device context.
+     * @param stepsPerRev The number of steps required to complete one
+     * full revolution.
+     * @param mode The stepper operating mode.  MD_STEP_MODE1 is more
+     * flexible, and is handled directly by this driver.
+     * MD_STEP_MODE2 is handled completely by the MCU on board the
+     * motor driver, but is much more restrictive and requires updated
+     * firmware.  We generally recommend MODE1.
+     */
+    void md_config_stepper(const md_context dev, unsigned int stepsPerRev,
+                           MD_STEP_MODE_T mode);
+
+
+#ifdef __cplusplus
+}
+#endif

src/md/md.hpp

@@ -1,6 +1,6 @@
 /*
  * Author: Jon Trulson <jtrulson@ics.com>
- * Copyright (c) 2014 Intel Corporation.
+ * Copyright (c) 2014-2016 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -23,14 +23,9 @@
  */
 #pragma once
 
-#include <stdint.h>
-#include <sys/time.h>
 #include <string>
-#include <mraa/types.hpp>
-#include <mraa/i2c.hpp>
 
-#define MD_I2C_BUS 0
+#include <md.h>
-#define MD_DEFAULT_I2C_ADDR 0x0f
 
 namespace upm {
     /**
@@ -83,40 +78,13 @@ namespace upm {
     class MD {
 
     public:
-    // MD registers
-    typedef enum { SET_SPEED           = 0x82,
-                   SET_PWM_FREQ        = 0x84,
-                   SET_DIRECTION       = 0xaa,
-                   SET_MOTOR_A         = 0xa1, // not documented
-                   SET_MOTOR_B         = 0xa5, // not documented
-                   STEPPER_ENABLE      = 0x1a, 
-                   STEPPER_DISABLE     = 0x1b,
-                   STEPPER_NUM_STEPS   = 0x1c
-    } REG_T;
-
-    // legal directions for the stepper
-    typedef enum { STEP_DIR_CCW    = 0x01,
-                   STEP_DIR_CW     = 0x00
-    } STEP_DIRECTION_T;
-    
-    // legal directions for individual DC motors
-    typedef enum { DIR_CCW    = 0x02,
-                   DIR_CW     = 0x01
-    } DC_DIRECTION_T;
-    
-    // stepper modes
-    typedef enum { STEP_MODE1 = 0x00,
-                   STEP_MODE2 = 0x01
-    } STEP_MODE_T;
-    
         /**
          * MD constructor
          *
          * @param bus I2C bus to use
          * @param address I2C address to use
          */
-    MD(int bus=MD_I2C_BUS,
+        MD(int bus=MD_I2C_BUS, uint8_t address=MD_DEFAULT_I2C_ADDR);
-            uint8_t address=MD_DEFAULT_I2C_ADDR);
 
         /**
          * MD destructor
@@ -131,7 +99,7 @@ namespace upm {
          * @param data2 Second byte of data
          * @return True if successful
          */
-    bool writePacket(REG_T reg, uint8_t data1, uint8_t data2);
+        bool writePacket(MD_REG_T reg, uint8_t data1, uint8_t data2);
 
         /**
          * To control DC motors, sets the speed of motors A & B.
@@ -161,7 +129,7 @@ namespace upm {
          * @param dirB Direction for motor B, DIR_CW or DIR_CCW
          * @return True if successful
          */
-    bool setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB);
+        bool setMotorDirections(MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB);
 
         /**
          * To control a stepper motor, sets its direction and speed, and
@@ -177,7 +145,7 @@ namespace upm {
          * will mean a slower speed.
          * @return True if successful
          */
-    bool enableStepper(STEP_DIRECTION_T dir, uint8_t speed);
+        bool enableStepper(MD_STEP_DIRECTION_T dir, uint8_t speed);
 
         /**
          * To control a stepper motor, stops the stepper motor.
@@ -208,48 +176,11 @@ namespace upm {
          * @param mode The stepper operating mode, default STEP_MODE1
          * @return Elapsed milliseconds
          */
-    void configStepper(unsigned int stepsPerRev, STEP_MODE_T mode=STEP_MODE1);
+        void configStepper(unsigned int stepsPerRev,
+                           MD_STEP_MODE_T mode=MD_STEP_MODE1);
 
     protected:
-    mraa::I2c m_i2c;
+        md_context m_md;
-    uint8_t m_addr;
-
-  private:
-    // steps per revolution
-    int m_stepsPerRev;
-    int m_currentStep;
-    uint32_t m_stepDelay;
-    uint32_t m_totalSteps;
-    STEP_MODE_T m_stepMode;
-
-    /**
-     * Steps the motor one tick
-     *
-     */
-    void stepperStep();
-
-    // step direction: - 1 = forward, -1 = backward
-    int m_stepDirection;
-
-    // This is a NOOP value used to pad packets
-    static const uint8_t MD_NOOP = 0x01;
-    // our timer
-    struct timeval m_startTime;
-
-    /**
-     * Returns the number of milliseconds elapsed since initClock()
-     * was last called.
-     *
-     * @return Elapsed milliseconds
-     */
-    uint32_t getMillis();
-
-    /**
-     * Resets the clock
-     *
-     */
-    void initClock();
-
     };
 }
 

src/md/pyupm_md.i

@@ -9,6 +9,7 @@
 %include "md_doc.i"
 #endif
 
+%include "md.h"
 %include "md.hpp"
 %{
     #include "md.hpp"