PWM based motor driver added from other project
This commit is contained in:
@@ -0,0 +1,311 @@
|
||||
/**
|
||||
* @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 IRAM_ATTR 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;
|
||||
}
|
||||
};
|
||||
Reference in New Issue
Block a user