alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-15 04:57:30 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

md: C implementation; C++ wraps C

Browse Source 
Signed-off-by: Jon Trulson <jtrulson@ics.com>
This commit is contained in:
Jon Trulson 2016-10-18 17:02:33 -06:00
parent df5b3805c5
commit 8ac8be9e0a
16 changed files with 813 additions and 397 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
examples/c++/md-stepper.cxx
View File

@ -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,
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();

9
examples/c++/md.cxx
View File

@ -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,
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]

2
examples/c/CMakeLists.txt
View File

@ -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)

69
examples/c/md-stepper.c Normal file
View File

@ -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;
}

64
examples/c/md.c Normal file
View File

@ -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;
}

8
examples/java/MDSample.java
View File

@ -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

4
examples/python/md-stepper.py
View File

@ -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()

6
examples/python/md.py
View File

@ -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")

6
src/md/CMakeLists.txt
View File

@ -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)

1
src/md/javaupm_md.i
View File

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

3
src/md/jsupm_md.i
View File

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

265
src/md/md.c Normal file
View File

@ -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;
}

206
src/md/md.cxx
View File

@ -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;
// 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;
}
}

220
src/md/md.h Normal file
View File

@ -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

337
src/md/md.hpp
View File

@ -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,234 +23,165 @@
*/
#pragma once
#include <stdint.h>
#include <sys/time.h>
#include <string>
#include <mraa/types.hpp>
#include <mraa/i2c.hpp>
#define MD_I2C_BUS 0
#define MD_DEFAULT_I2C_ADDR 0x0f
#include <md.h>
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;
};
}

1
src/md/pyupm_md.i
View File

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