129 lines
2.7 KiB
C++
129 lines
2.7 KiB
C++
#include "src/drivers/stepper.h"
|
|
#include "src/math/Utilities.h"
|
|
|
|
using namespace math;
|
|
|
|
namespace drivers {
|
|
|
|
Stepper::Stepper(PinName stepPin,
|
|
PinName dirPin,
|
|
PinName enPin)
|
|
: m_step(stepPin)
|
|
, m_dir(dirPin)
|
|
, m_en(enPin)
|
|
, m_accelerationLimitOn(true)
|
|
, m_accelerationLimit(50.0f)
|
|
, m_stepsPerRevolution(200)
|
|
, m_microStepResolution(8)
|
|
, m_currentPeriod(0)
|
|
, m_configuredDirection(1)
|
|
, m_lastDirection(1)
|
|
, m_latestSpeed(0.0f)
|
|
{
|
|
m_step.period_us(1000000);
|
|
m_step.pulsewidth_us(5.0f);
|
|
|
|
// Start controller deactivated
|
|
m_en.write(1);
|
|
m_dir.write(m_configuredDirection);
|
|
}
|
|
|
|
void Stepper::init()
|
|
{
|
|
|
|
}
|
|
|
|
void Stepper::enable()
|
|
{
|
|
// Controller is enabled when IO is low
|
|
m_en.write(0);
|
|
}
|
|
|
|
void Stepper::disable()
|
|
{
|
|
// Controller is disabled when IO is high
|
|
m_en.write(1);
|
|
}
|
|
|
|
bool Stepper::isEnabled()
|
|
{
|
|
// Controller is enabled when IO is low
|
|
return m_en.read() == 0;
|
|
}
|
|
|
|
void Stepper::setDirection(int dir)
|
|
{
|
|
m_lastDirection = -1; // Deem invalid
|
|
m_configuredDirection = dir;
|
|
m_dir.write(dir == 1 ? 1 : 0);
|
|
}
|
|
|
|
int Stepper::getDirection()
|
|
{
|
|
return m_configuredDirection;
|
|
}
|
|
|
|
float Stepper::limitAcceleration(float DPS)
|
|
{
|
|
float delta = DPS - m_latestSpeed;
|
|
|
|
if (abs(delta) > m_accelerationLimit)
|
|
{
|
|
return (delta > 0) ?
|
|
m_latestSpeed + m_accelerationLimit :
|
|
m_latestSpeed - m_accelerationLimit;
|
|
}
|
|
return DPS;
|
|
}
|
|
|
|
void Stepper::setSpeed(const float& DPS)
|
|
{
|
|
m_latestSpeed = limitAcceleration(DPS);
|
|
|
|
float revPerSecond = abs(m_latestSpeed)/(float)360.0;
|
|
revPerSecond = constrain(revPerSecond, 100.0f);
|
|
|
|
float stepsPerSecond =
|
|
m_stepsPerRevolution*m_microStepResolution*revPerSecond;
|
|
|
|
float usPerSecond = 1000000.0f;
|
|
|
|
// Some high value to make motors "stop" | And to avoid division by zero
|
|
float periodUs = (stepsPerSecond == 0.0) ? 100000.0 : usPerSecond/stepsPerSecond;
|
|
|
|
// Precaution. Don't know how close the period can be to
|
|
// the pulsewidth or if it will cause any issues
|
|
if (periodUs < (float)100.0f)
|
|
{
|
|
periodUs = (float)100.0f;
|
|
}
|
|
if (periodUs > (float)100000.0f)
|
|
{
|
|
periodUs = (float)100000.0f;
|
|
}
|
|
|
|
// No need to set anything new
|
|
if (periodUs != m_currentPeriod)
|
|
{
|
|
m_step.period_us(periodUs);
|
|
m_step.pulsewidth_us(5.0f);
|
|
}
|
|
m_currentPeriod = periodUs;
|
|
|
|
int dir = (m_latestSpeed > (float)0.0f) ? 1 : -1;
|
|
|
|
if (m_lastDirection != dir)
|
|
{
|
|
// Give dir to motor controller
|
|
m_dir.write((m_configuredDirection*dir == 1) ? 1 : 0);
|
|
|
|
m_lastDirection = dir;
|
|
}
|
|
}
|
|
|
|
float Stepper::getSpeed()
|
|
{
|
|
return m_latestSpeed;
|
|
}
|
|
|
|
} // namespace drivers
|