alexey.zholtikov/upm
1
0
Fork 0
mirror of https://github.com/eclipse/upm.git synced 2025-03-24 01:10:22 +03:00
Code Issues Actions Packages Projects Releases Wiki Activity

stepmotor: made significant changes to stepper driver

Browse Source 
Fun to work on, I have maybe 3 different implementations for this one now.
Submitted version uses GPIOs only, no more PWM thus enhancing compatibility.
Fast writes and busy-wait delays ensure accuracy to a few μs when generating
the step pulses.

Signed-off-by: Mihai Tudor Panu <mihai.tudor.panu@intel.com>
This commit is contained in:
Mihai Tudor Panu 2015-12-15 11:00:05 -08:00
parent 74691914fb
commit c8e80bf6a3
4 changed files with 220 additions and 101 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
docs/images/stepmotor.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 52 KiB

After

Width:  |  Height:  |  Size: 198 KiB

49
examples/c++/stepmotor.cxx
View File

@ -1,5 +1,6 @@
/* /*
* Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com> * Authors: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
* Mihai Tudor Panu <mihai.tudor.panu@intel.com>
* Copyright (c) 2014 Intel Corporation. * Copyright (c) 2014 Intel Corporation.
* *
* Permission is hereby granted, free of charge, to any person obtaining * 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. * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/ */
#include <string.h>
#include <unistd.h> #include <unistd.h>
#include <iostream> #include <iostream>
#include "stepmotor.h"
#include <signal.h> #include <signal.h>
#include "stepmotor.h"
int doWork = 0; using namespace std;
int doWork = 1;
upm::StepMotor *sensor = NULL; upm::StepMotor *sensor = NULL;
void void
@ -37,7 +39,7 @@ sig_handler(int signo)
printf("got signal\n"); printf("got signal\n");
if (signo == SIGINT) { if (signo == SIGINT) {
printf("exiting application\n"); printf("exiting application\n");
doWork = 1; doWork = 0;
} }
} }
@ -45,26 +47,29 @@ int
main(int argc, char **argv) main(int argc, char **argv)
{ {
//! [Interesting] //! [Interesting]
sensor = new upm::StepMotor(4, 6); sensor = new upm::StepMotor(2, 3);
while (!doWork) { while (doWork) {
sensor->setSpeed (500); cout << "1 Revolution forward and back at 60 rpm" << endl;
sensor->stepForward (500); sensor->setSpeed (60);
usleep (10000); sensor->stepForward(200);
sensor->stepBackwards (500); usleep (1000000);
usleep (10000); sensor->stepBackwards(200);
usleep (1000000);
sensor->setSpeed (750); cout << "1 Revolution forward and back at 150 rpm" << endl;
sensor->stepForward (500); sensor->setSpeed (150);
usleep (10000); sensor->stepForward(200);
sensor->stepBackwards (500); usleep (1000000);
usleep (10000); sensor->stepBackwards(200);
usleep (1000000);
sensor->setSpeed (1000); cout << "1 Revolution forward and back at 300 rpm" << endl;
sensor->stepForward (500); sensor->setSpeed (300);
usleep (10000); sensor->stepForward (200);
sensor->stepBackwards (500); usleep (1000000);
usleep (10000); sensor->stepBackwards (200);
usleep (1000000);
} }
delete sensor; delete sensor;

154
src/stepmotor/stepmotor.cxx
View File

@ -1,5 +1,6 @@
/* /*
* Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com> * Authors: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
* Mihai Tudor Panu <mihai.tudor.panu@intel.com>
* Copyright (c) 2014 Intel Corporation. * Copyright (c) 2014 Intel Corporation.
* *
* Permission is hereby granted, free of charge, to any person obtaining * Permission is hereby granted, free of charge, to any person obtaining
@ -25,86 +26,157 @@
#include <iostream> #include <iostream>
#include <string> #include <string>
#include <stdexcept> #include <stdexcept>
#include <unistd.h>
#include <stdlib.h> #include <stdlib.h>
#include <time.h>
#include "stepmotor.h" #include "stepmotor.h"
using namespace upm; using namespace upm;
using namespace std;
StepMotor::StepMotor (int dirPin, int stePin) StepMotor::StepMotor (int dirPin, int stePin, int steps, int enPin)
: m_pwmStepContext(stePin), m_dirPinCtx(dirPin) { : m_dirPinCtx(dirPin), m_stePinCtx(stePin), m_enPinCtx(0), m_steps(steps) {
mraa::Result error = mraa::SUCCESS;
m_name = "StepMotor"; m_name = "StepMotor";
setSpeed(60);
setStep(0);
m_stePin = stePin; if (m_dirPinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) {
m_dirPin = dirPin; 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 (m_stePinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) {
if (error != mraa::SUCCESS) { throw std::runtime_error(string(__FUNCTION__) +
mraa::printError (error); ": 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 void
StepMotor::setSpeed (int speed) { StepMotor::setSpeed (int speed) {
if (speed > MAX_PERIOD) { if (speed > 0) {
m_speed = MAX_PERIOD; m_delay = 60000000 / (speed * m_steps);
} else {
throw std::invalid_argument(string(__FUNCTION__) +
": Parameter must be greater than 0");
} }
}
if (speed < MIN_PERIOD) { mraa::Result
m_speed = MIN_PERIOD; StepMotor::step (int ticks) {
if (ticks < 0) {
return stepBackwards(abs(ticks));
} else {
return stepForward(ticks);
} }
m_speed = speed;
} }
mraa::Result mraa::Result
StepMotor::stepForward (int ticks) { StepMotor::stepForward (int ticks) {
dirForward (); dirForward();
return move (ticks); 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 mraa::Result
StepMotor::stepBackwards (int ticks) { StepMotor::stepBackwards (int ticks) {
dirBackwards (); dirBackwards();
return move (ticks); 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 void
StepMotor::move (int ticks) { StepMotor::setStep (int step) {
mraa::Result error = mraa::SUCCESS; if (step <= m_steps) {
m_position = step;
m_pwmStepContext.enable (1);
for (int tick = 0; tick < ticks; tick++) {
m_pwmStepContext.period_us (m_speed);
m_pwmStepContext.pulsewidth_us (PULSEWIDTH);
} }
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 mraa::Result
StepMotor::dirForward () { StepMotor::dirForward () {
mraa::Result error = mraa::SUCCESS; mraa::Result error = m_dirPinCtx.write(HIGH);
error = m_dirPinCtx.write (HIGH);
if (error != mraa::SUCCESS) { if (error != mraa::SUCCESS) {
mraa::printError (error); throw std::runtime_error(string(__FUNCTION__) +
": Could not write to dirPin");
} }
return error; return error;
} }
mraa::Result mraa::Result
StepMotor::dirBackwards () { StepMotor::dirBackwards () {
mraa::Result error = mraa::SUCCESS; mraa::Result error = m_dirPinCtx.write(LOW);
error = m_dirPinCtx.write (LOW);
if (error != mraa::SUCCESS) { if (error != mraa::SUCCESS) {
mraa::printError (error); throw std::runtime_error(string(__FUNCTION__) +
": Could not write to dirPin");
} }
return error; 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;
}
}

118
src/stepmotor/stepmotor.h
View File

@ -1,5 +1,6 @@
/* /*
* Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com> * Authors: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com>
* Mihai Tudor Panu <mihai.tudor.panu@intel.com>
* Copyright (c) 2014 Intel Corporation. * Copyright (c) 2014 Intel Corporation.
* *
* Credits to Adafruit. * Credits to Adafruit.
@ -27,25 +28,21 @@
#pragma once #pragma once
#include <string> #include <string>
#include <math.h>
#include <mraa/pwm.hpp> #include <mraa/pwm.hpp>
#include <mraa/aio.hpp>
#include <mraa/common.hpp> #include <mraa/common.hpp>
#include <mraa/gpio.hpp> #include <mraa/gpio.hpp>
#define MIN_PERIOD 500 #define OVERHEAD_US 6
#define MAX_PERIOD 1000 #define MINPULSE_US 5
#define PULSEWIDTH 480
#define HIGH 1 #define HIGH 1
#define LOW 0 #define LOW 0
namespace upm { namespace upm {
/** /**
* @brief Stepper Motor library * @brief Stepper Motor library
* @defgroup stepper libupm-stepper * @defgroup stepmotor libupm-stepmotor
* @ingroup seeed sparkfun pwm gpio motor * @ingroup sparkfun generic gpio motor
*/ */
/** /**
* @library stepmotor * @library stepmotor
@ -53,70 +50,115 @@ namespace upm {
* @comname Stepper Motor * @comname Stepper Motor
* @altname EasyDriver Stepper Motor Driver * @altname EasyDriver Stepper Motor Driver
* @type motor * @type motor
* @man seeed sparkfun * @man sparkfun generic
* @web http://www.schmalzhaus.com/EasyDriver/index.html * @web http://www.schmalzhaus.com/EasyDriver/index.html
* @con pwm gpio * @con gpio
* *
* @brief API for the Stepper Motor * @brief API for the Stepper Motor
* *
* This module defines the Stepper Motor interface. It is compatible with stepper * This module defines the Stepper Motor interface. It is compatible with
* motor drivers that use 2 pins to control the motor, like an Easy Driver * stepper motor drivers that use 2 pins to control the motor, like an Easy
* from Brian Schmalz. * 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 * @image html stepmotor.jpg
* <br><em>ECS1030 Sensor image provided by SparkFun* under
* <a href=https://creativecommons.org/licenses/by-nc-sa/3.0/>
* CC BY-NC-SA-3.0</a>.</em>
*
* @snippet stepmotor.cxx Interesting * @snippet stepmotor.cxx Interesting
*/ */
class StepMotor { class StepMotor {
public: public:
/** /**
* Instantiates a StepMotor object * Instantiates a StepMotor object.
* *
* @param dirPin Direction GPIO pin * @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 * StepMotor object destructor.
* no need for the destructor; all the connections will be */
* closed when m_dirPinCtx and m_pwmStepContext go out of ~StepMotor ();
* scope
* ~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); 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); 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); 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: private:
std::string m_name; std::string m_name;
int m_dirPin; mraa::Gpio m_dirPinCtx;
int m_stePin; mraa::Gpio m_stePinCtx;
int m_speed; mraa::Gpio *m_enPinCtx;
mraa::Gpio m_dirPinCtx; int m_delay;
mraa::Pwm m_pwmStepContext; int m_steps;
int m_position;
mraa::Result move (int ticks);
mraa::Result dirForward (); mraa::Result dirForward ();
mraa::Result dirBackwards (); mraa::Result dirBackwards ();
void move ();
void delayus (int us);
}; };
} }