mirror of
https://github.com/eclipse/upm.git
synced 2025-03-15 04:57:30 +03:00
stepmotor: made significant changes to stepper driver
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
parent
74691914fb
commit
c8e80bf6a3
Binary file not shown.
|
Before Width: | Height: | Size: 52 KiB After Width: | Height: | Size: 198 KiB |
@ -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.
|
||||
*
|
||||
* 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 <string.h>
|
||||
#include <unistd.h>
|
||||
#include <iostream>
|
||||
#include "stepmotor.h"
|
||||
#include <signal.h>
|
||||
#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;
|
||||
|
||||
@ -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.
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining
|
||||
@ -25,86 +26,157 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <stdexcept>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <time.h>
|
||||
#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;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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.
|
||||
*
|
||||
* Credits to Adafruit.
|
||||
@ -27,25 +28,21 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <math.h>
|
||||
#include <mraa/pwm.hpp>
|
||||
#include <mraa/aio.hpp>
|
||||
#include <mraa/common.hpp>
|
||||
|
||||
#include <mraa/gpio.hpp>
|
||||
|
||||
#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
|
||||
* <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
|
||||
*/
|
||||
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);
|
||||
};
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user