diff --git a/examples/c++/md-stepper.cxx b/examples/c++/md-stepper.cxx index 6a8a6076..976dea64 100644 --- a/examples/c++/md-stepper.cxx +++ b/examples/c++/md-stepper.cxx @@ -1,6 +1,6 @@ /* * Author: Jon Trulson - * 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, - MD_DEFAULT_I2C_ADDR); + upm::MD *motors = new upm::MD(MD_I2C_BUS, 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(); diff --git a/examples/c++/md.cxx b/examples/c++/md.cxx index 9a03f094..8c028224 100644 --- a/examples/c++/md.cxx +++ b/examples/c++/md.cxx @@ -1,6 +1,6 @@ /* * Author: Jon Trulson - * 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, - MD_DEFAULT_I2C_ADDR); + upm::MD *motors = new upm::MD(MD_I2C_BUS, 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] diff --git a/examples/c/CMakeLists.txt b/examples/c/CMakeLists.txt index c33cfb94..683b3771 100644 --- a/examples/c/CMakeLists.txt +++ b/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) diff --git a/examples/c/md-stepper.c b/examples/c/md-stepper.c new file mode 100644 index 00000000..7f832c82 --- /dev/null +++ b/examples/c/md-stepper.c @@ -0,0 +1,69 @@ +/* + * Author: Jon Trulson + * 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 +#include + +#include +#include + +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; +} diff --git a/examples/c/md.c b/examples/c/md.c new file mode 100644 index 00000000..88393246 --- /dev/null +++ b/examples/c/md.c @@ -0,0 +1,64 @@ +/* + * Author: Jon Trulson + * 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 +#include + +#include +#include + + +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; +} diff --git a/examples/java/MDSample.java b/examples/java/MDSample.java index db4802ae..f635c270 100644 --- a/examples/java/MDSample.java +++ b/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, - 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.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, - 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.MD_DIR_CCW); Thread.sleep(3000); // stop motors diff --git a/examples/python/md-stepper.py b/examples/python/md-stepper.py index 8251ba1e..3c26b0e0 100755 --- a/examples/python/md-stepper.py +++ b/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() diff --git a/examples/python/md.py b/examples/python/md.py index 23d3151a..20bc037f 100755 --- a/examples/python/md.py +++ b/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, - upmmd.MD.DIR_CCW) + myMotorDriver.setMotorDirections(upmmd.MD_DIR_CCW, + upmmd.MD_DIR_CCW) time.sleep(3) print("Stopping motors") diff --git a/src/md/CMakeLists.txt b/src/md/CMakeLists.txt index 186839fc..8470a722 100644 --- a/src/md/CMakeLists.txt +++ b/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) diff --git a/src/md/javaupm_md.i b/src/md/javaupm_md.i index e40d9d1d..fe39565c 100644 --- a/src/md/javaupm_md.i +++ b/src/md/javaupm_md.i @@ -5,6 +5,7 @@ #include "md.hpp" %} +%include "md.h" %include "md.hpp" %pragma(java) jniclasscode=%{ diff --git a/src/md/jsupm_md.i b/src/md/jsupm_md.i index 08229256..0fad6a15 100644 --- a/src/md/jsupm_md.i +++ b/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" diff --git a/src/md/md.c b/src/md/md.c new file mode 100644 index 00000000..21b85808 --- /dev/null +++ b/src/md/md.c @@ -0,0 +1,265 @@ +/* + * Author: Jon Trulson + * 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 +#include +#include + +#include + +#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; +} diff --git a/src/md/md.cxx b/src/md/md.cxx index ee9a6fe3..f9a636c0 100644 --- a/src/md/md.cxx +++ b/src/md/md.cxx @@ -1,6 +1,6 @@ /* * Author: Jon Trulson - * 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; - - // 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"); - return; - } - - initClock(); - // default to mode1 stepper operation, 200 steps per rev. - configStepper(200, STEP_MODE1); + if (!m_md) + throw std::runtime_error(std::string(__FUNCTION__) + + ": md_init() failed"); } MD::~MD() { - setMotorSpeeds(0, 0); - writePacket(SET_DIRECTION, 0, MD_NOOP); + md_close(m_md); } -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)) + throw std::runtime_error(std::string(__FUNCTION__) + + ": md_write_packet() failed"); - 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"); - 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; + 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"); + + return true; } -bool MD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB) +bool MD::setMotorDirections(MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB) { - uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03); - return writePacket(SET_DIRECTION, dir, MD_NOOP); + 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(STEP_DIRECTION_T dir, uint8_t speed) +bool MD::enableStepper(MD_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; + return md_enable_stepper(m_md, dir, speed); } bool MD::disableStepper() { - if (m_stepMode == STEP_MODE2) - return writePacket(STEPPER_DISABLE, MD_NOOP, MD_NOOP); - - // else, mode 1 - writePacket(SET_DIRECTION, 0, MD_NOOP); - return setMotorSpeeds(0, 0); + return md_disable_stepper(m_md); } bool MD::setStepperSteps(unsigned int steps) { - if (m_stepMode == STEP_MODE2) - { - 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; + return md_set_stepper_steps(m_md, steps); } -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; - } -} diff --git a/src/md/md.h b/src/md/md.h new file mode 100644 index 00000000..0c4790ca --- /dev/null +++ b/src/md/md.h @@ -0,0 +1,220 @@ +/* + * Author: Jon Trulson + * 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 +#include + +#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 diff --git a/src/md/md.hpp b/src/md/md.hpp index f5599b60..f9bbf72d 100644 --- a/src/md/md.hpp +++ b/src/md/md.hpp @@ -1,6 +1,6 @@ /* * Author: Jon Trulson - * 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,234 +23,165 @@ */ #pragma once -#include -#include #include -#include -#include -#define MD_I2C_BUS 0 -#define MD_DEFAULT_I2C_ADDR 0x0f +#include namespace upm { - /** - * @brief I2C Motor Driver library - * @defgroup md libupm-md - * @ingroup seeed i2c motor robok - */ - - /** - * @library md - * @sensor md - * @comname I2C Motor Driver - * @altname Grove Motor Driver - * @type motor - * @man seeed - * @con i2c - * @kit robok - * - * @brief API for the I2C Motor Driver - * - * This class implements support for the I2C Motor Driver. - * This device can support a single 4-wire stepper motor, or two - * 2-wire DC motors. The device contains an Atmel* ATmega8L - * microcontroller that manages an L298N H-bridge driver chip. - * - * This device supports an I2C bus speed of 100Khz only. - * - * The module does not provide any telemetry or status - it only - * accepts I2C commands for its various operations. - * - * This module was tested with version 1.3 of the I2C Motor - * Driver. - * - * For stepper operation, this driver can run in one of two modes - - * Mode 1, where this driver handles the stepping operation, and - * Mode 2, where this driver simply sends commands to the - * Motor Driver, and it handles the stepping operation. Mode2 - * requires updated (and working) firmware to be loaded onto the - * device. - * - * The default stepper operation mode is Mode1, which is generally - * more flexible and is supported on all firmware revisions. - * - * @image html md.jpg - * An example showing the use of a DC motor - * @snippet md.cxx Interesting - * An example showing the use of a 4-wire stepper - * @snippet md-stepper.cxx Interesting - */ - 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 + * @brief I2C Motor Driver library + * @defgroup md libupm-md + * @ingroup seeed i2c motor robok */ - MD(int bus=MD_I2C_BUS, - uint8_t address=MD_DEFAULT_I2C_ADDR); /** - * MD destructor - */ - ~MD(); - - /** - * Composes and writes a 3-byte packet to the controller + * @library md + * @sensor md + * @comname I2C Motor Driver + * @altname Grove Motor Driver + * @type motor + * @man seeed + * @con i2c + * @kit robok * - * @param reg Register location - * @param data1 First byte of data - * @param data2 Second byte of data - * @return True if successful - */ - bool writePacket(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. + * @brief API for the I2C Motor Driver * - * @param speedA Speed of motor A - * @param speedB Speed of motor B - * @return True if successful - */ - bool setMotorSpeeds(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. + * This class implements support for the I2C Motor Driver. + * This device can support a single 4-wire stepper motor, or two + * 2-wire DC motors. The device contains an Atmel* ATmega8L + * microcontroller that manages an L298N H-bridge driver chip. * - * @param freq PWM prescale frequency; default is 0x03 - * @return True if successful - */ - bool setPWMFrequencyPrescale(uint8_t freq=0x03); - - /** - * To control DC motors, sets the directions of motors A & B + * This device supports an I2C bus speed of 100Khz only. * - * @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 successful - */ - bool setMotorDirections(DC_DIRECTION_T dirA, 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 setStepperSteps() has - * completed. + * The module does not provide any telemetry or status - it only + * accepts I2C commands for its various operations. * - * @param dir Direction, STEP_DIR_CW or 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 enableStepper(STEP_DIRECTION_T dir, uint8_t speed); - - /** - * To control a stepper motor, stops the stepper motor. + * This module was tested with version 1.3 of the I2C Motor + * Driver. * - * @return True if successful - */ - bool disableStepper(); - - /** - * 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 stepper operation, this driver can run in one of two modes - + * Mode 1, where this driver handles the stepping operation, and + * Mode 2, where this driver simply sends commands to the + * Motor Driver, and it handles the stepping operation. Mode2 + * requires updated (and working) firmware to be loaded onto the + * device. * - * For Mode1 (the default) steps can be any positive integer. + * The default stepper operation mode is Mode1, which is generally + * more flexible and is supported on all firmware revisions. * - * @param steps Number of steps to execute. 255 (only in Mode2) - * means continuous rotation. - * @return True if successful + * @image html md.jpg + * An example showing the use of a DC motor + * @snippet md.cxx Interesting + * An example showing the use of a 4-wire stepper + * @snippet md-stepper.cxx Interesting */ - bool setStepperSteps(unsigned int steps); + class MD { - /** - * Configure the initial Stepper parameters. This should be - * called before any other stepper method. - * - * @param stepsPerRev The number of steps required to complete one - * full revolution. - * @param mode The stepper operating mode, default STEP_MODE1 - * @return Elapsed milliseconds - */ - void configStepper(unsigned int stepsPerRev, STEP_MODE_T mode=STEP_MODE1); + public: + /** + * MD constructor + * + * @param bus I2C bus to use + * @param address I2C address to use + */ + MD(int bus=MD_I2C_BUS, uint8_t address=MD_DEFAULT_I2C_ADDR); - protected: - mraa::I2c m_i2c; - uint8_t m_addr; + /** + * MD destructor + */ + ~MD(); - private: - // steps per revolution - int m_stepsPerRev; - int m_currentStep; - uint32_t m_stepDelay; - uint32_t m_totalSteps; - STEP_MODE_T m_stepMode; + /** + * Composes and writes a 3-byte packet to the controller + * + * @param reg Register location + * @param data1 First byte of data + * @param data2 Second byte of data + * @return True if successful + */ + bool writePacket(MD_REG_T reg, uint8_t data1, uint8_t data2); - /** - * Steps the motor one tick - * - */ - void stepperStep(); + /** + * To control DC motors, sets the speed of motors A & B. + * Valid values are 0-255. + * + * @param speedA Speed of motor A + * @param speedB Speed of motor B + * @return True if successful + */ + bool setMotorSpeeds(uint8_t speedA, uint8_t speedB); - // step direction: - 1 = forward, -1 = backward - int m_stepDirection; + /** + * 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 freq PWM prescale frequency; default is 0x03 + * @return True if successful + */ + bool setPWMFrequencyPrescale(uint8_t freq=0x03); - // This is a NOOP value used to pad packets - static const uint8_t MD_NOOP = 0x01; - // our timer - struct timeval m_startTime; + /** + * To control DC motors, sets the directions of motors A & B + * + * @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 successful + */ + bool setMotorDirections(MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB); - /** - * Returns the number of milliseconds elapsed since initClock() - * was last called. - * - * @return Elapsed milliseconds - */ - uint32_t getMillis(); + /** + * 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 setStepperSteps() has + * completed. + * + * @param dir Direction, STEP_DIR_CW or 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 enableStepper(MD_STEP_DIRECTION_T dir, uint8_t speed); - /** - * Resets the clock - * - */ - void initClock(); + /** + * To control a stepper motor, stops the stepper motor. + * + * @return True if successful + */ + bool disableStepper(); - }; + /** + * 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 steps Number of steps to execute. 255 (only in Mode2) + * means continuous rotation. + * @return True if successful + */ + bool setStepperSteps(unsigned int steps); + + /** + * Configure the initial Stepper parameters. This should be + * called before any other stepper method. + * + * @param stepsPerRev The number of steps required to complete one + * full revolution. + * @param mode The stepper operating mode, default STEP_MODE1 + * @return Elapsed milliseconds + */ + void configStepper(unsigned int stepsPerRev, + MD_STEP_MODE_T mode=MD_STEP_MODE1); + + protected: + md_context m_md; + }; } diff --git a/src/md/pyupm_md.i b/src/md/pyupm_md.i index fc7c01e4..069b1db3 100644 --- a/src/md/pyupm_md.i +++ b/src/md/pyupm_md.i @@ -9,6 +9,7 @@ %include "md_doc.i" #endif +%include "md.h" %include "md.hpp" %{ #include "md.hpp"