From d380658b405dbd97c5fe26c0c3f0015f2b6a0c9a Mon Sep 17 00:00:00 2001 From: Jon Trulson Date: Fri, 23 Jan 2015 16:30:22 -0700 Subject: [PATCH] grovemd: Initial implementation This module implements support for the Grove I2C Motor Driver. The device *requires* a 100Khz I2C bus speed. It will not work on anything faster. Signed-off-by: Jon Trulson Signed-off-by: Zion Orent Signed-off-by: Mihai Tudor Panu --- examples/c++/CMakeLists.txt | 3 + examples/c++/grovemd.cxx | 60 ++++++++++++ examples/javascript/grovemd.js | 92 ++++++++++++++++++ src/grovemd/CMakeLists.txt | 5 + src/grovemd/grovemd.cxx | 138 ++++++++++++++++++++++++++ src/grovemd/grovemd.h | 173 +++++++++++++++++++++++++++++++++ src/grovemd/jsupm_grovemd.i | 8 ++ src/grovemd/pyupm_grovemd.i | 13 +++ 8 files changed, 492 insertions(+) create mode 100644 examples/c++/grovemd.cxx create mode 100644 examples/javascript/grovemd.js create mode 100644 src/grovemd/CMakeLists.txt create mode 100644 src/grovemd/grovemd.cxx create mode 100644 src/grovemd/grovemd.h create mode 100644 src/grovemd/jsupm_grovemd.i create mode 100644 src/grovemd/pyupm_grovemd.i diff --git a/examples/c++/CMakeLists.txt b/examples/c++/CMakeLists.txt index ae80b472..558cac3b 100644 --- a/examples/c++/CMakeLists.txt +++ b/examples/c++/CMakeLists.txt @@ -97,6 +97,7 @@ add_executable (ina132-example ina132.cxx) add_executable (l298-example l298.cxx) add_executable (l298-stepper-example l298-stepper.cxx) add_executable (at42qt1070-example at42qt1070.cxx) +add_executable (grovemd-example grovemd.cxx) include_directories (${PROJECT_SOURCE_DIR}/src/hmc5883l) include_directories (${PROJECT_SOURCE_DIR}/src/grove) @@ -175,6 +176,7 @@ include_directories (${PROJECT_SOURCE_DIR}/src/grovegsr) include_directories (${PROJECT_SOURCE_DIR}/src/ina132) include_directories (${PROJECT_SOURCE_DIR}/src/l298) include_directories (${PROJECT_SOURCE_DIR}/src/at42qt1070) +include_directories (${PROJECT_SOURCE_DIR}/src/grovemd) target_link_libraries (hmc5883l-example hmc5883l ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (groveled-example grove ${CMAKE_THREAD_LIBS_INIT}) @@ -273,3 +275,4 @@ target_link_libraries (ina132-example ina132 ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (l298-example l298 ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (l298-stepper-example l298 ${CMAKE_THREAD_LIBS_INIT}) target_link_libraries (at42qt1070-example at42qt1070 ${CMAKE_THREAD_LIBS_INIT}) +target_link_libraries (grovemd-example grovemd ${CMAKE_THREAD_LIBS_INIT}) diff --git a/examples/c++/grovemd.cxx b/examples/c++/grovemd.cxx new file mode 100644 index 00000000..497b87de --- /dev/null +++ b/examples/c++/grovemd.cxx @@ -0,0 +1,60 @@ +/* + * Author: Jon Trulson + * Copyright (c) 2014 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 "grovemd.h" + +using namespace std; + +int main(int argc, char **argv) +{ + //! [Interesting] + // Instantiate an I2C Grove Motor Driver on I2C bus 0 + + upm::GroveMD *motors = new upm::GroveMD(GROVEMD_I2C_BUS, + GROVEMD_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->setDirection(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); + sleep(3); + + //! [Interesting] + + cout << "Stopping motors" << endl; + motors->setMotorSpeeds(0, 0); + + cout << "Exiting..." << endl; + + delete motors; + return 0; +} diff --git a/examples/javascript/grovemd.js b/examples/javascript/grovemd.js new file mode 100644 index 00000000..693f0b63 --- /dev/null +++ b/examples/javascript/grovemd.js @@ -0,0 +1,92 @@ +/*jslint node:true, vars:true, bitwise:true, unparam:true */ +/*jshint unused:true */ + +/* +* Author: Zion Orent +* Copyright (c) 2015 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. +*/ + +var groveMotorDriver_lib = require('jsupm_grovemd'); + +function start() +{ + if (my_MotorDriver_obj) + { + // 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.setMotorSpeeds(127, 127); + } +} + +function reverse() +{ + if (my_MotorDriver_obj) + { + // counter clockwise + console.log("Reversing M1 and M2 for 3 seconds"); + my_MotorDriver_obj.setDirection(groveMotorDriver_lib.GroveMD.DIR_CCW); + } +} + +function end() +{ + if (my_MotorDriver_obj) + { + console.log("Stopping motors"); + my_MotorDriver_obj.setMotorSpeeds(0, 0); + } + exit(); +} + +// When exiting: clear memory and print exit message +function exit() +{ + if (my_MotorDriver_obj) + { + my_MotorDriver_obj = null; + groveMotorDriver_lib.cleanUp(); + } + groveMotorDriver_lib = null; + console.log("Exiting"); + process.exit(0); +} + + +// Instantiate an I2C Grove Motor Driver on I2C bus 0 +var my_MotorDriver_obj = new groveMotorDriver_lib.GroveMD( + groveMotorDriver_lib.GROVEMD_I2C_BUS, + groveMotorDriver_lib.GROVEMD_DEFAULT_I2C_ADDR); + +start(); + +setTimeout(function() +{ + reverse(); + setTimeout(end, 3000); +}, 3000); + + +process.on('SIGINT', function() +{ + exit(); +}); diff --git a/src/grovemd/CMakeLists.txt b/src/grovemd/CMakeLists.txt new file mode 100644 index 00000000..6f307151 --- /dev/null +++ b/src/grovemd/CMakeLists.txt @@ -0,0 +1,5 @@ +set (libname "grovemd") +set (libdescription "upm grove i2c motor driver module") +set (module_src ${libname}.cxx) +set (module_h ${libname}.h) +upm_module_init() diff --git a/src/grovemd/grovemd.cxx b/src/grovemd/grovemd.cxx new file mode 100644 index 00000000..2fa59598 --- /dev/null +++ b/src/grovemd/grovemd.cxx @@ -0,0 +1,138 @@ +/* + * Author: Jon Trulson + * Copyright (c) 2014 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 "grovemd.h" + +using namespace upm; +using namespace std; + + +GroveMD::GroveMD(int bus, uint8_t address) +{ + m_addr = address; + + // setup our i2c link + if ( !(m_i2c = mraa_i2c_init(bus)) ) + { + cerr << __FUNCTION__ << ": mraa_i2c_init() failed" << endl; + return; + } + + // this board *requires* 100Khz i2c bus only + mraa_result_t rv; + if ( (rv = mraa_i2c_frequency(m_i2c, MRAA_I2C_STD)) != MRAA_SUCCESS ) + { + cerr << "GroveMD: Could not set i2c frequency (MRAA_I2C_STD). " << endl; + mraa_result_print(rv); + return; + } + + if (mraa_i2c_address(m_i2c, m_addr)) + { + cerr << "GroveMD: Could not initialize i2c bus. " << endl; + return; + } +} + +GroveMD::~GroveMD() +{ + setMotorSpeeds(0, 0); + mraa_i2c_stop(m_i2c); +} + +bool GroveMD::writePacket(REG_T reg, uint8_t data1, uint8_t data2) +{ + uint8_t buf[3]; + + buf[0] = reg; + buf[1] = data1; + buf[2] = data2; + + mraa_result_t rv; + if ( (rv = mraa_i2c_address(m_i2c, m_addr)) != MRAA_SUCCESS ) + { + cerr << __FUNCTION__ << ": mraa_i2c_address() failed. " << endl; + mraa_result_print(rv); + 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); + + if ( (rv = mraa_i2c_write(m_i2c, buf, 3)) != MRAA_SUCCESS ) + { + cerr << __FUNCTION__ << ": mraa_i2c_write() failed:" << endl; + mraa_result_print(rv); + return false; + } + + return true; +} + +bool GroveMD::setMotorSpeeds(uint8_t speedA, uint8_t speedB) +{ + return writePacket(SET_SPEED, speedA, speedB); +} + +bool GroveMD::setPWMFrequencyPrescale(uint8_t freq) +{ + return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP); +} + +bool GroveMD::setDirection(DIRECTION_T dir) +{ + return writePacket(SET_DIRECTION, dir, GROVEMD_NOOP); +} + +bool GroveMD::enableStepper(DIRECTION_T dir, uint8_t speed) +{ + return writePacket(STEPPER_ENABLE, dir, speed); +} + +bool GroveMD::disableStepper() +{ + return writePacket(STEPPER_DISABLE, GROVEMD_NOOP, GROVEMD_NOOP); +} + +bool GroveMD::setStepperSteps(uint8_t steps) +{ + if (steps == 0) + { + // invalid + cerr << __FUNCTION__ << ": invalid number of steps. " + << "Valid values are between 1 and 255." << endl; + return false; + } + + return writePacket(STEPPER_NUM_STEPS, steps, GROVEMD_NOOP); +} + diff --git a/src/grovemd/grovemd.h b/src/grovemd/grovemd.h new file mode 100644 index 00000000..5d6f887d --- /dev/null +++ b/src/grovemd/grovemd.h @@ -0,0 +1,173 @@ +/* + * Author: Jon Trulson + * Copyright (c) 2014 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 GROVEMD_I2C_BUS 0 +#define GROVEMD_DEFAULT_I2C_ADDR 0x0f + +// This is a NOOP value used to pad packets +#define GROVEMD_NOOP 0x01 + +namespace upm { + /** + * @brief UPM module for the Grove I2C Motor Driver + * @defgroup grovemd libupm-grovemd + * @ingroup seeed i2c motor + */ + + /** + * @sensor grovemd + * @library grovemd + * @name Grove I2C Motor Driver + * @category motor + * @manufacturer seeed + * @connection i2c + * + * @brief C++ API for the Grove I2C Motor Driver + * + * This class implements support for the Grove 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 Grove I2C Motor + * Driver + * + * @ingroup i2c grove + * @snippet grovemd.cxx Interesting + */ + class GroveMD { + + public: + // GroveMD 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 + typedef enum { DIR_CCW = 0x0a, + DIR_CW = 0x05 + } DIRECTION_T; + + /** + * grovemd constructor + * + * @param bus i2c bus to use + * @param address i2c address to use + */ + GroveMD(int bus=GROVEMD_I2C_BUS, + uint8_t address=GROVEMD_DEFAULT_I2C_ADDR); + + /** + * GroveMD Destructor + */ + ~GroveMD(); + + /** + * Compose and write 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 write successful + */ + bool writePacket(REG_T reg, uint8_t data1, uint8_t data2); + + /** + * For controlling DC motors, set the speeds of motors A & B. + * Valid values are 0-255. + * + * @param speedA speed of motor A + * @param speedB speed of motor B + * @return true if command successful + */ + bool setMotorSpeeds(uint8_t speedA, uint8_t speedB); + + /** + * For controlling DC motors, set the PWM frequency prescale + * factor. Note this register is not ducumented other than to say + * that the default value is 0x03. Presumably this is the timer + * pre-scale factor used on the ATMega MCU timer driving the PWM. + * + * @param freq PWM prescale frequency, default 0x03 + * @return true if command successful + */ + bool setPWMFrequencyPrescale(uint8_t freq=0x03); + + /** + * For controlling DC motors, set the direction + * + * @param dir direction, CW or CCW + * @return true if command successful + */ + bool setDirection(DIRECTION_T dir); + + /** + * For controlling a stepper motor, set a direction, speed and + * then enable. + * + * @param dir direction, CW or 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); + + /** + * For controlling a stepper motor, stop the stepper motor. + * + * @return true if command successful + */ + bool disableStepper(); + + /** + * For controlling a stepper motor, specify the number of steps to + * execute. Valid values are 1-255, 255 means to rotate continuously. + * + * @param steps number of steps to execute. 255 means rotate continously. + * @return true if command successful + */ + bool setStepperSteps(uint8_t steps); + + + private: + mraa_i2c_context m_i2c; + uint8_t m_addr; + }; +} + + diff --git a/src/grovemd/jsupm_grovemd.i b/src/grovemd/jsupm_grovemd.i new file mode 100644 index 00000000..a907c05b --- /dev/null +++ b/src/grovemd/jsupm_grovemd.i @@ -0,0 +1,8 @@ +%module jsupm_grovemd +%include "../upm.i" + +%{ + #include "grovemd.h" +%} + +%include "grovemd.h" diff --git a/src/grovemd/pyupm_grovemd.i b/src/grovemd/pyupm_grovemd.i new file mode 100644 index 00000000..ccc908b3 --- /dev/null +++ b/src/grovemd/pyupm_grovemd.i @@ -0,0 +1,13 @@ +%module pyupm_grovemd +%include "../upm.i" + +%feature("autodoc", "3"); + +#ifdef DOXYGEN +%include "grovemd_doc.i" +#endif + +%include "grovemd.h" +%{ + #include "grovemd.h" +%}