313 lines
8.8 KiB
C++
313 lines
8.8 KiB
C++
/**
|
|
* @file stepperMotor.hpp
|
|
* @author Gustice
|
|
* @brief Stepper Motor device class
|
|
*
|
|
* @copyright Copyright (c) 2024
|
|
*/
|
|
#pragma once
|
|
|
|
#include "gpTimer.hpp"
|
|
#include "gpioPort.hpp"
|
|
#include "stepperDrv.hpp"
|
|
#include <Task.hpp>
|
|
#include <cmath>
|
|
#include <memory>
|
|
|
|
struct Profile {
|
|
std::vector<uint8_t> steps;
|
|
};
|
|
|
|
class StepperMotor {
|
|
enum Mode {
|
|
Idle = 0,
|
|
HomingFromSensor, // if home sensor already active
|
|
HomingToSensorBegin,
|
|
HomingToSensorEnd,
|
|
Homing,
|
|
Run,
|
|
CrawlToEnd,
|
|
};
|
|
|
|
public:
|
|
template <int MAX, int PREC>
|
|
struct Position {
|
|
uint16_t position{};
|
|
uint16_t subPos{};
|
|
Position &operator+(Position &lhs) {
|
|
auto v1 = getFixPoint();
|
|
auto v2 = lhs.getFixPoint();
|
|
auto v = v1 + v2;
|
|
if (v > (MAX << 16)) {
|
|
v -= (MAX << 16);
|
|
}
|
|
position = getPosition(v);
|
|
subPos = getSubPos(v);
|
|
return *this;
|
|
}
|
|
|
|
Position &operator-(Position &lhs) {
|
|
auto v1 = getFixPoint();
|
|
auto v2 = lhs.getFixPoint();
|
|
auto v = v1 - v2;
|
|
if (v < (MAX << 16)) {
|
|
v += (MAX << 16);
|
|
}
|
|
position = getPosition(v);
|
|
subPos = getSubPos(v);
|
|
return *this;
|
|
}
|
|
|
|
Position &operator+(StepperDriver::StepCplx &pos) {
|
|
position += pos.increment;
|
|
if (position > MAX) {
|
|
position -= MAX;
|
|
} else if (position < 0) {
|
|
position += MAX;
|
|
} else {
|
|
// nothing to do here
|
|
}
|
|
subPos += pos.subPos;
|
|
return *this;
|
|
}
|
|
|
|
Position &halv() {
|
|
auto v1 = getFixPoint();
|
|
auto v = v1 / 2;
|
|
position = getPosition(v);
|
|
subPos = getSubPos(v);
|
|
return *this;
|
|
}
|
|
|
|
float getScaled() {
|
|
auto scaled = (float)position / MAX;
|
|
// scaled += (float)subPos / (1 << PREC);
|
|
return scaled;
|
|
}
|
|
|
|
private:
|
|
int32_t getFixPoint() {
|
|
int32_t value = (((int32_t)position) << 16) | (subPos << (16 - PREC));
|
|
return value;
|
|
}
|
|
uint16_t getPosition(int32_t value) {
|
|
auto pos = (value >> 16) & 0xFFFF;
|
|
return pos;
|
|
}
|
|
uint16_t getSubPos(int32_t value) {
|
|
auto pos = ((value & 0xFFFF) >> (16 - PREC));
|
|
return pos;
|
|
}
|
|
};
|
|
|
|
using Trajectory = std::unique_ptr<Profile>;
|
|
|
|
struct Config {
|
|
Config(float stepDeg, int microsteps)
|
|
: step(stepDeg), resolution(microsteps), resolutionQuad(resolution/4) {}
|
|
|
|
/// @brief Step-size in degree (e.g. 1.8 deg)
|
|
float step;
|
|
/// @brief Step-size in radiant (e.g. 1.8 deg)
|
|
float stepRad{(float)(std::numbers::pi * step / 180)};
|
|
/// @brief Number of microsteps for four steps
|
|
int resolution;
|
|
/// @brief Number of microsteps for four steps
|
|
int resolutionQuad;
|
|
/// @brief Number of Steps for full revolution
|
|
int steps{std::lround(360.0F / step)};
|
|
};
|
|
|
|
StepperMotor(Config &config, StepperDriver &driver, InputPort &initSensor)
|
|
: _config(config), _driver(driver), _initSensor(initSensor),
|
|
_timer(TimerCallParam, *this) {
|
|
_timer.setup(1'000, HwTimerBase::Mode::Periodic);
|
|
_timer.start();
|
|
}
|
|
~StepperMotor() {};
|
|
|
|
void findRef() {
|
|
ESP_LOGI("Stepper", "Homing ...");
|
|
if (_initSensor.read()) {
|
|
_mode = Mode::HomingFromSensor;
|
|
ESP_LOGI("Stepper", "First leave HomePos");
|
|
} else {
|
|
_mode = Mode::HomingToSensorBegin;
|
|
ESP_LOGI("Stepper", "Find start of HomePos");
|
|
}
|
|
|
|
while (_mode != Mode::Idle) {
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
|
|
int fullStep = _config.resolution / 4;
|
|
_driver.setPos(0);
|
|
|
|
if (_initSensor.read()) {
|
|
ESP_LOGI("Stepper", "Need to move away from home");
|
|
// MoveAway
|
|
while (_initSensor.read()) {
|
|
(void)_driver.step(-fullStep);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
for (size_t i = 0; i < 8; i++) // Drive another 4 Steps away
|
|
{
|
|
(void)_driver.step(-fullStep);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
}
|
|
|
|
ESP_LOGI("Stepper", "Fast find right side of home sensor");
|
|
// Find right Endpoint
|
|
while (not _initSensor.read()) {
|
|
(void)_driver.step(fullStep);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
while (_initSensor.read()) { // move away
|
|
(void)_driver.step(-fullStep);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
ESP_LOGI("Stepper", "Find right side of home sensor");
|
|
while (not _initSensor.read()) {
|
|
(void)_driver.step(1);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
int pos{};
|
|
resetPosition();
|
|
ESP_LOGI("Stepper", "Find end of sensor");
|
|
while (_initSensor.read()) {
|
|
(void)_driver.step(1);
|
|
pos++;
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
for (size_t i = 0; i < 32; i++) // Drive another 4 Steps away
|
|
{
|
|
(void)_driver.step(1);
|
|
pos++;
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
|
|
ESP_LOGI("Stepper", "Find left side of home sensor");
|
|
while (not _initSensor.read()) {
|
|
(void)_driver.step(-1);
|
|
pos--;
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
|
|
ESP_LOGI("Stepper", "Move back to mid position %d", pos / 2);
|
|
for (size_t i = 0; i < pos / 2; i++) {
|
|
(void)_driver.step(-1);
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
ESP_LOGI("Stepper", "Reset position");
|
|
resetPosition();
|
|
}
|
|
|
|
void demoFullTurn() {
|
|
auto steps = _config.resolution * _config.steps;
|
|
ESP_LOGI("Stepper", "Running %i * %i steps for full turn", _config.resolution, _config.steps);
|
|
|
|
ESP_LOGI("Stepper", "Start is %f.2", getScaledPos());
|
|
for (size_t i = 0; i < _config.resolutionQuad * _config.steps; i++) {
|
|
auto delta = _driver.step(1);
|
|
_indexPos = _indexPos + delta;
|
|
fos::Task::delayTicks(1);
|
|
}
|
|
ESP_LOGI("Stepper", "End is %f.2", getScaledPos());
|
|
}
|
|
|
|
void createDemoProfile();
|
|
void moveBy(float angle);
|
|
void moveBy(float angle, uint8_t profile, float accel, float speed);
|
|
|
|
void runProfile(Trajectory profile) {
|
|
if (profile == nullptr) {
|
|
return;
|
|
}
|
|
|
|
_trajectory = std::move(profile);
|
|
_step = 0;
|
|
_mode = Run;
|
|
}
|
|
|
|
bool isIdle() {
|
|
return _mode == Mode::Idle;
|
|
}
|
|
|
|
float getScaledPos() {
|
|
return _indexPos.getScaled();
|
|
}
|
|
|
|
void resetPosition() {
|
|
_indexPos = Position<50, 5>();
|
|
}
|
|
|
|
private:
|
|
Config &_config;
|
|
StepperDriver &_driver;
|
|
InputPort &_initSensor;
|
|
HwTimerT<StepperMotor &> _timer;
|
|
Mode _mode{};
|
|
int _step{};
|
|
Trajectory _trajectory{};
|
|
Position<50, 5> _indexPos{};
|
|
|
|
static bool TimerCallParam(HwTimerT<StepperMotor &> &, StepperMotor &stepper) {
|
|
switch (stepper._mode) {
|
|
case Mode::HomingFromSensor: {
|
|
if (stepper._initSensor.read() == false) {
|
|
stepper._mode = HomingToSensorBegin;
|
|
break;
|
|
}
|
|
(void)stepper._driver.step(1);
|
|
} break;
|
|
|
|
case Mode::HomingToSensorBegin: {
|
|
if (stepper._initSensor.read()) {
|
|
stepper._mode = HomingToSensorEnd;
|
|
stepper._indexPos = {0, stepper._driver.getSubPos()};
|
|
break;
|
|
}
|
|
(void)stepper._driver.step(1);
|
|
} break;
|
|
|
|
case Mode::HomingToSensorEnd: {
|
|
if (stepper._initSensor.read() == false) {
|
|
stepper._mode = Homing;
|
|
stepper._indexPos = stepper._indexPos.halv();
|
|
break;
|
|
}
|
|
auto delta = stepper._driver.step(1);
|
|
stepper._indexPos = stepper._indexPos + delta;
|
|
} break;
|
|
|
|
case Mode::Homing: {
|
|
if (stepper._indexPos.position == 0) {
|
|
stepper._mode = Idle;
|
|
break;
|
|
}
|
|
auto delta = stepper._driver.step(-1);
|
|
stepper._indexPos = stepper._indexPos + delta;
|
|
} break;
|
|
|
|
case Mode::Run: {
|
|
auto stp = stepper._trajectory->steps[stepper._step];
|
|
auto delta = stepper._driver.setPos(stp);
|
|
stepper._indexPos = stepper._indexPos + delta;
|
|
|
|
stepper._step++;
|
|
if (stepper._step >= stepper._trajectory->steps.size()) {
|
|
stepper._mode = Idle;
|
|
}
|
|
} break;
|
|
|
|
case Mode::Idle:
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
};
|