diff --git a/docs/images/stepmotor.jpg b/docs/images/stepmotor.jpg index 25dbe1d0..c6a5cfc0 100755 Binary files a/docs/images/stepmotor.jpg and b/docs/images/stepmotor.jpg differ diff --git a/examples/c++/stepmotor.cxx b/examples/c++/stepmotor.cxx index cc8091ea..4986f438 100644 --- a/examples/c++/stepmotor.cxx +++ b/examples/c++/stepmotor.cxx @@ -1,5 +1,6 @@ /* - * Author: Yevgeniy Kiveisha + * Authors: Yevgeniy Kiveisha + * Mihai Tudor Panu * Copyright (c) 2014 Intel Corporation. * * Permission is hereby granted, free of charge, to any person obtaining @@ -22,13 +23,14 @@ * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -#include #include #include -#include "stepmotor.h" #include +#include "stepmotor.h" -int doWork = 0; +using namespace std; + +int doWork = 1; upm::StepMotor *sensor = NULL; void @@ -37,7 +39,7 @@ sig_handler(int signo) printf("got signal\n"); if (signo == SIGINT) { printf("exiting application\n"); - doWork = 1; + doWork = 0; } } @@ -45,26 +47,29 @@ int main(int argc, char **argv) { //! [Interesting] - sensor = new upm::StepMotor(4, 6); + sensor = new upm::StepMotor(2, 3); - while (!doWork) { - sensor->setSpeed (500); - sensor->stepForward (500); - usleep (10000); - sensor->stepBackwards (500); - usleep (10000); + while (doWork) { + cout << "1 Revolution forward and back at 60 rpm" << endl; + sensor->setSpeed (60); + sensor->stepForward(200); + usleep (1000000); + sensor->stepBackwards(200); + usleep (1000000); - sensor->setSpeed (750); - sensor->stepForward (500); - usleep (10000); - sensor->stepBackwards (500); - usleep (10000); + cout << "1 Revolution forward and back at 150 rpm" << endl; + sensor->setSpeed (150); + sensor->stepForward(200); + usleep (1000000); + sensor->stepBackwards(200); + usleep (1000000); - sensor->setSpeed (1000); - sensor->stepForward (500); - usleep (10000); - sensor->stepBackwards (500); - usleep (10000); + cout << "1 Revolution forward and back at 300 rpm" << endl; + sensor->setSpeed (300); + sensor->stepForward (200); + usleep (1000000); + sensor->stepBackwards (200); + usleep (1000000); } delete sensor; diff --git a/src/stepmotor/stepmotor.cxx b/src/stepmotor/stepmotor.cxx index 6cb98d11..a84bcd66 100644 --- a/src/stepmotor/stepmotor.cxx +++ b/src/stepmotor/stepmotor.cxx @@ -1,5 +1,6 @@ /* - * Author: Yevgeniy Kiveisha + * Authors: Yevgeniy Kiveisha + * Mihai Tudor Panu * Copyright (c) 2014 Intel Corporation. * * Permission is hereby granted, free of charge, to any person obtaining @@ -25,86 +26,157 @@ #include #include #include -#include #include - +#include #include "stepmotor.h" using namespace upm; +using namespace std; -StepMotor::StepMotor (int dirPin, int stePin) - : m_pwmStepContext(stePin), m_dirPinCtx(dirPin) { - mraa::Result error = mraa::SUCCESS; +StepMotor::StepMotor (int dirPin, int stePin, int steps, int enPin) + : m_dirPinCtx(dirPin), m_stePinCtx(stePin), m_enPinCtx(0), m_steps(steps) { m_name = "StepMotor"; + setSpeed(60); + setStep(0); - m_stePin = stePin; - m_dirPin = dirPin; + if (m_dirPinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) { + throw std::runtime_error(string(__FUNCTION__) + + ": Could not initialize dirPin as output"); + return; + } + m_dirPinCtx.useMmap(true); + m_dirPinCtx.write(0); - error = m_dirPinCtx.dir (mraa::DIR_OUT); - if (error != mraa::SUCCESS) { - mraa::printError (error); + if (m_stePinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) { + throw std::runtime_error(string(__FUNCTION__) + + ": Could not initialize stePin as output"); + return; + } + m_stePinCtx.useMmap(true); + m_stePinCtx.write(0); + + if (enPin >= 0) { + m_enPinCtx = new mraa::Gpio(enPin); + if(m_enPinCtx->dir(mraa::DIR_OUT) != mraa::SUCCESS) { + throw std::runtime_error(string(__FUNCTION__) + + ": Could not initialize enPin as output"); + return; + } + m_enPinCtx->useMmap(true); + enable(true); + } +} + +StepMotor::~StepMotor () { + if (m_enPinCtx) + delete m_enPinCtx; +} + +void +StepMotor::enable (bool flag) { + if (m_enPinCtx) { + m_enPinCtx->write(flag); + } else { + throw std::runtime_error(string(__FUNCTION__) + + ": Enable pin not defined"); } } void StepMotor::setSpeed (int speed) { - if (speed > MAX_PERIOD) { - m_speed = MAX_PERIOD; + if (speed > 0) { + m_delay = 60000000 / (speed * m_steps); + } else { + throw std::invalid_argument(string(__FUNCTION__) + + ": Parameter must be greater than 0"); } +} - if (speed < MIN_PERIOD) { - m_speed = MIN_PERIOD; +mraa::Result +StepMotor::step (int ticks) { + if (ticks < 0) { + return stepBackwards(abs(ticks)); + } else { + return stepForward(ticks); } - - m_speed = speed; } mraa::Result StepMotor::stepForward (int ticks) { - dirForward (); - return move (ticks); + dirForward(); + for (int i = 0; i < ticks; i++) { + move(); + if (++m_position >= m_steps) { + m_position = 0; + } + delayus(m_delay - MINPULSE_US - OVERHEAD_US); + } + return mraa::SUCCESS; } mraa::Result StepMotor::stepBackwards (int ticks) { - dirBackwards (); - return move (ticks); + dirBackwards(); + for (int i = 0; i < ticks; i++) { + move(); + if (--m_position < 0) { + m_position = m_steps - 1; + } + delayus(m_delay - MINPULSE_US - OVERHEAD_US); + } + return mraa::SUCCESS; } -mraa::Result -StepMotor::move (int ticks) { - mraa::Result error = mraa::SUCCESS; - - m_pwmStepContext.enable (1); - for (int tick = 0; tick < ticks; tick++) { - m_pwmStepContext.period_us (m_speed); - m_pwmStepContext.pulsewidth_us (PULSEWIDTH); +void +StepMotor::setStep (int step) { + if (step <= m_steps) { + m_position = step; } - m_pwmStepContext.enable (0); +} - return error; +int +StepMotor::getStep () { + return m_position; +} + +void +StepMotor::move () { + m_stePinCtx.write(1); + delayus(MINPULSE_US); + m_stePinCtx.write(0); } mraa::Result StepMotor::dirForward () { - mraa::Result error = mraa::SUCCESS; - - error = m_dirPinCtx.write (HIGH); + mraa::Result error = m_dirPinCtx.write(HIGH); if (error != mraa::SUCCESS) { - mraa::printError (error); + throw std::runtime_error(string(__FUNCTION__) + + ": Could not write to dirPin"); } - return error; } mraa::Result StepMotor::dirBackwards () { - mraa::Result error = mraa::SUCCESS; - - error = m_dirPinCtx.write (LOW); + mraa::Result error = m_dirPinCtx.write(LOW); if (error != mraa::SUCCESS) { - mraa::printError (error); + throw std::runtime_error(string(__FUNCTION__) + + ": Could not write to dirPin"); } - return error; } + +void upm::StepMotor::delayus (int us) { + int diff = 0; + struct timespec gettime_now; + + clock_gettime(CLOCK_REALTIME, &gettime_now); + int start = gettime_now.tv_nsec; + while (diff < us * 1000) + { + clock_gettime(CLOCK_REALTIME, &gettime_now); + diff = gettime_now.tv_nsec - start; + if (diff < 0) + diff += 1000000000; + } +} diff --git a/src/stepmotor/stepmotor.h b/src/stepmotor/stepmotor.h index f745b5f0..c77ed658 100644 --- a/src/stepmotor/stepmotor.h +++ b/src/stepmotor/stepmotor.h @@ -1,5 +1,6 @@ /* - * Author: Yevgeniy Kiveisha + * Authors: Yevgeniy Kiveisha + * Mihai Tudor Panu * Copyright (c) 2014 Intel Corporation. * * Credits to Adafruit. @@ -27,25 +28,21 @@ #pragma once #include -#include #include -#include #include - #include -#define MIN_PERIOD 500 -#define MAX_PERIOD 1000 -#define PULSEWIDTH 480 +#define OVERHEAD_US 6 +#define MINPULSE_US 5 -#define HIGH 1 -#define LOW 0 +#define HIGH 1 +#define LOW 0 namespace upm { /** * @brief Stepper Motor library - * @defgroup stepper libupm-stepper - * @ingroup seeed sparkfun pwm gpio motor + * @defgroup stepmotor libupm-stepmotor + * @ingroup sparkfun generic gpio motor */ /** * @library stepmotor @@ -53,70 +50,115 @@ namespace upm { * @comname Stepper Motor * @altname EasyDriver Stepper Motor Driver * @type motor - * @man seeed sparkfun + * @man sparkfun generic * @web http://www.schmalzhaus.com/EasyDriver/index.html - * @con pwm gpio + * @con gpio * * @brief API for the Stepper Motor - * - * This module defines the Stepper Motor interface. It is compatible with stepper - * motor drivers that use 2 pins to control the motor, like an Easy Driver - * from Brian Schmalz. + * + * This module defines the Stepper Motor interface. It is compatible with + * stepper motor drivers that use 2 pins to control the motor, like an Easy + * Driver from Brian Schmalz or the STR driver series from Applied Motion. It + * can also control an enable pin if one is available and connected. + * + * The implementation is synchronous and thus blocking while the stepper motor + * is in motion. However it is possible to send the commands via threading and + * the performance of the library will be very good given a low CPU load. On a + * busy system though you will notice some jitter especially at higher speeds. + * It is possible to reduce this effect to some extent by using smoothing + * and/or microstepping on stepper drivers that support such features. * * @image html stepmotor.jpg + *
ECS1030 Sensor image provided by SparkFun* under + * + * CC BY-NC-SA-3.0. + * * @snippet stepmotor.cxx Interesting */ class StepMotor { public: /** - * Instantiates a StepMotor object + * Instantiates a StepMotor object. * * @param dirPin Direction GPIO pin - * @param stePin Stepper pulse PWM pin + * @param stePin Stepper pulse GPIO pin + * @param steps Number of steps per revolution (Default 200) + * @param enPin Enable pin if connected (Optional) */ - StepMotor (int dirPin, int stePin); + StepMotor (int dirPin, int stePin, int steps = 200, int enPin = -1); /** - * StepMotor object destructor - * no need for the destructor; all the connections will be - * closed when m_dirPinCtx and m_pwmStepContext go out of - * scope - * ~StepMotor (); - **/ + * StepMotor object destructor. + */ + ~StepMotor (); /** - * Sets the rotation speed + * Can be used to enable/disable the stepper driver if an enable pin is + * available and connected. Check your data sheet as some drivers might + * have the enable logic inverted. * - * @param speed Rotation speed + * @param flag true to enable or false to disable + */ + void enable (bool flag); + + /** + * Sets the rotation speed in rpm. Default 60 rpm. + * + * @param speed Rotation speed in rpm */ void setSpeed (int speed); /** - * Rotates the motor forward + * Rotates the motor by the specified number of steps. Positive values + * rotate clockwise and negative values rotate counter-clockwise. * - * @param ticks Number of ticks the motor moves + * @param ticks Number of steps the motor moves + */ + mraa::Result step (int ticks); + + /** + * Rotates the motor forward (clockwise). + * + * @param ticks Number of steps the motor moves */ mraa::Result stepForward (int ticks); /** - * Rotates the motor backward + * Rotates the motor backward (counter-clockwise). * - * @param ticks Number of ticks the motor moves + * @param ticks Number of steps the motor moves */ mraa::Result stepBackwards (int ticks); + /** + * Sets the current step. Useful if the motor is not at 0 when the + * driver is initialized. + * + * @param step Current shaft position + */ + void setStep (int step); + + /** + * Gets the current step. + * + * @return Current shaft position. + */ + int getStep (); + private: std::string m_name; - int m_dirPin; - int m_stePin; - int m_speed; + mraa::Gpio m_dirPinCtx; + mraa::Gpio m_stePinCtx; + mraa::Gpio *m_enPinCtx; - mraa::Gpio m_dirPinCtx; - mraa::Pwm m_pwmStepContext; + int m_delay; + int m_steps; + int m_position; - mraa::Result move (int ticks); mraa::Result dirForward (); mraa::Result dirBackwards (); + void move (); + void delayus (int us); }; }