julien.oculi/technoshop.laser_turret_debug
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity
technoshop.laser_turret_debug/include/kissStepper.h

313 lines
9.9 KiB
C++
Raw Blame History

/*
kissStepper - a lightweight library for the Easy Driver, Big Easy Driver, Allegro stepper motor drivers and others that use a Step/Dir interface
Written by Rylee Isitt. September 21, 2015
License: GNU Lesser General Public License (LGPL) V2.1
Despite the existence of several excellent libraries for driving stepper motors, I created this one to fulfill the following needs:
- Simplicity
- Handling of enable, step, and dir pins
- Based around an external loop
- Approximately linear acceleration using a fast algorithm
- High step frequency (or reasonably so, given the overhead involved)
- Use AVR/ARM libraries and port access to increase performance while keeping the API Arduino-friendly
- Teensy (Teensyduino) compatibility
Acceleration approximation math is based on Aryeh Eiderman's "Real Time Stepper Motor Linear Ramping Just by Addition and Multiplication", available at http://hwml.com/LeibRamp.pdf
*/
#ifndef kissStepper_H
#define kissStepper_H
#include <Arduino.h>
// determine port register size
#if defined(__AVR__) || defined(__avr__)
typedef uint8_t regint;
#elif defined(TEENSYDUINO)
#if defined(__AVR_ATmega32U4__) || defined(__AVR_AT90USB1286__) || defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MKL26Z64__) || defined(__MK64FX512__) || defined(__MK66FX1M0__)
typedef uint8_t regint;
#else
typedef uint32_t regint;
#endif
#else
typedef uint32_t regint;
#endif
// the order of enums allows some simple tests:
// if > STATE_STARTING, motor is in motion
// if > STATE_RUN, motor is accelerating or decelerating
enum kissState_t: uint8_t
{
STATE_STOPPED = 0,
STATE_STARTING = 1,
STATE_RUN = 2,
STATE_ACCEL = 3,
STATE_DECEL = 4
};
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// kissStepper without acceleration
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
class kissStepperNoAccel
{
public:
kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
kissStepperNoAccel(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
~kissStepperNoAccel(void) {};
bool prepareMove(int32_t target);
kissState_t move(void);
void stop(void);
uint16_t getCurSpeed(void)
{
if (m_kissState == STATE_RUN)
return m_maxSpeed;
else
return 0;
}
kissState_t getState(void)
{
return m_kissState;
}
int32_t getPos(void)
{
if (m_forwards)
return m_pos + m_distMoved;
else
return m_pos - m_distMoved;
}
bool isEnabled(void)
{
return m_enabled;
}
bool isMovingForwards(void)
{
return m_forwards;
}
void begin(void);
void enable(void);
void disable(void);
void setPos(int32_t pos)
{
if (m_kissState == STATE_STOPPED)
m_pos = constrain(pos, m_reverseLimit, m_forwardLimit);
}
int32_t getTarget(void)
{
if (m_kissState == STATE_STOPPED)
return m_pos;
else if (m_forwards)
return m_pos + m_distTotal;
else
return m_pos - m_distTotal;
}
uint32_t getDistRemaining(void)
{
return m_distTotal - m_distMoved;
}
void setForwardLimit(int32_t forwardLimit)
{
m_forwardLimit = forwardLimit;
}
void setReverseLimit(int32_t reverseLimit)
{
m_reverseLimit = reverseLimit;
}
int32_t getForwardLimit(void)
{
return m_forwardLimit;
}
int32_t getReverseLimit(void)
{
return m_reverseLimit;
}
void setMaxSpeed(uint16_t maxSpeed)
{
if (m_kissState == STATE_STOPPED) m_maxSpeed = maxSpeed;
}
uint16_t getMaxSpeed(void)
{
return m_maxSpeed;
}
protected:
void setDir(bool forwards)
{
m_forwards = forwards;
digitalWrite(PIN_DIR, forwards == m_invertDir);
}
void updatePos(void)
{
if (m_forwards)
m_pos += m_distMoved;
else
m_pos -= m_distMoved;
m_distMoved = 0;
}
static const uint32_t ONE_SECOND = 1000000UL;
static const uint8_t PULSE_WIDTH_US = 2; // desired width of step pulse (high) in us
static const int32_t DEFAULT_FORWARD_LIMIT = 2147483647L;
static const int32_t DEFAULT_REVERSE_LIMIT = -2147483648L;
static const uint16_t DEFAULT_SPEED = 1600;
static const uint16_t INTERVAL_CORRECTION_INCREMENT = 255;
int32_t m_forwardLimit;
int32_t m_reverseLimit;
uint16_t m_maxSpeed;
const uint8_t PIN_DIR;
const uint8_t PIN_STEP;
const uint8_t PIN_ENABLE;
kissState_t m_kissState;
uint32_t m_distTotal, m_distMoved;
bool m_forwards;
int32_t m_pos;
const regint m_stepBit;
regint volatile * const m_stepOut;
uint32_t m_stepIntervalWhole;
uint16_t m_stepIntervalRemainder;
uint16_t m_stepIntervalCorrectionCounter;
bool m_enabled;
uint32_t m_lastStepTime;
bool m_invertDir;
bool m_init;
};
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// kissStepper WITH acceleration
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------------------
class kissStepper: public kissStepperNoAccel
{
public:
kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, uint8_t PIN_ENABLE = 255, bool invertDir = false);
kissStepper(uint8_t PIN_DIR, uint8_t PIN_STEP, bool invertDir = false);
~kissStepper(void) {};
bool prepareMove(int32_t target);
kissState_t move(void);
void stop(void);
uint16_t getCurSpeed(void)
{
if (m_kissState == STATE_RUN)
return m_maxSpeed;
else if (m_kissState > STATE_STARTING)
{
uint32_t curSpeed = ONE_SECOND / m_stepIntervalWhole;
if (curSpeed > m_maxSpeed) curSpeed = m_maxSpeed;
return curSpeed;
}
else
return 0;
}
void decelerate(void);
uint32_t calcMaxAccelDist(void)
{
if (m_accel > 0)
return ((uint32_t)m_maxSpeed * m_maxSpeed) / (2UL * m_accel);
else
return 0;
}
uint32_t getAccelDist(void)
{
return m_distAccel;
}
uint32_t getRunDist(void)
{
return m_distRun - m_distAccel;
}
uint32_t getDecelDist(void)
{
return m_distTotal - m_distRun;
}
void setAccel(uint16_t accel)
{
if (m_kissState == STATE_STOPPED) m_accel = accel;
}
uint16_t getAccel(void)
{
return m_accel;
}
uint16_t getTopSpeed(void);
protected:
static const uint16_t DEFAULT_ACCEL = 1600;
uint32_t m_distAccel, m_distRun;
uint32_t m_topSpeedStepInterval;
uint32_t m_minSpeedStepInterval;
float m_stepInterval;
float m_constMult;
uint16_t m_accel;
private:
/*
----------------------------------------------------------------------------------------------------
To strike a balance between accuracy and performance, this library uses a set of approximations
for calculating stepInterval when accelerating/decelerating. Although this does use floating point
math, it is a drastic improvement over exact calculations and better than anything else I've tried.
There is probably room for further improvement (fixed point or integer math?) but this is good enough.
exact:
stepInterval = ONE_SECOND / newSpeed
curSpeed = ONE_SECOND / stepInterval
newSpeed = sqrt(curSpeed^2 + 2a)
stepInterval = ONE_SECOND / sqrt(curSpeed^2 + 2a)
approximations:
constMult = accel / (ONE_SECOND * ONE_SECOND)
q = constMult*stepInterval*stepInterval
set q to negative if accelerating
good precision, fast: stepInterval *= 1.0 + q
better precision, slower: stepInterval *= 1.0 + q + q*q
best precision, slowest: stepInterval *= 1.0 + q + 1.5*q*q
----------------------------------------------------------------------------------------------------
*/
float accelStep(float stepInterval, float constMult)
{
float newStepInterval;
float q = -constMult*stepInterval*stepInterval;
newStepInterval = stepInterval * (1.0 + q);
// newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
// newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
if (newStepInterval < m_topSpeedStepInterval) newStepInterval = m_topSpeedStepInterval;
return newStepInterval;
}
float decelStep(float stepInterval, float constMult)
{
float newStepInterval;
float q = constMult*stepInterval*stepInterval;
newStepInterval = stepInterval * (1.0 + q);
// newStepInterval = stepInterval * (1.0 + q + q*q); // better accuracy
// newStepInterval = stepInterval * (1.0 + q + 1.5*q*q); // best accuracy
if (newStepInterval > m_minSpeedStepInterval) newStepInterval = m_minSpeedStepInterval;
return newStepInterval;
}
};
#endif
Reference in a new issue View git blame Copy permalink