288 lines
8.4 KiB
C++
288 lines
8.4 KiB
C++
/**
|
|
* @brief stepperMotor.cpp
|
|
* @author Gustice
|
|
* @brief Stepper Motor device class
|
|
*
|
|
* @copyright Copyright (c) 2024
|
|
*/
|
|
|
|
#include "stepperMotor.hpp"
|
|
#include "esp_log.h"
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <vector>
|
|
|
|
class ProfileGenerator {
|
|
public:
|
|
ProfileGenerator(StepperMotor::Config &config, float dT, float a, float w)
|
|
: _config(config), _dT(dT), _aMax(a), _wMax(w) {
|
|
}
|
|
|
|
Profile createSmoothStep(float delta) {
|
|
init();
|
|
// already solved in simple step
|
|
smoothSubStep(delta, _aMax, _wMax);
|
|
return transform();
|
|
}
|
|
|
|
Profile createBuildUpStep(float delta) {
|
|
init();
|
|
|
|
// t0..t1: increasing swing
|
|
buildUpSwing(_aMax / 4, _wMax, 3);
|
|
auto pnt = getStopDistance(_aMax, _wMax);
|
|
|
|
// t1..t2: movement to until almost destination
|
|
moveUntil(_wMax, delta - pnt.p);
|
|
// t2..r3: deceleration until destination
|
|
accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });
|
|
|
|
return transform();
|
|
}
|
|
|
|
Profile createRapidStartSwingInEnd(float delta) {
|
|
init();
|
|
|
|
auto pnt = getStopDistance(_aMax, _wMax);
|
|
auto wMax = _wMax;
|
|
// t0..t1: acceleration to max velocity
|
|
accelUntil(_aMax * 2, 0, [wMax](Point pnt) { return pnt.w > (wMax * 2); });
|
|
// t1..r2: deceleration until moving speed
|
|
accelUntil(-_aMax, _wMax * 2, [wMax](Point pnt) { return pnt.w < wMax; });
|
|
// t2..t3: movement to until destination (override)
|
|
moveUntil(_wMax, delta);
|
|
// t3..t4: deceleration
|
|
accelUntil(-_aMax, _wMax, [](Point pnt) { return pnt.w < 0; });
|
|
|
|
float pCen = delta + (_pVec.back() - delta) / 2;
|
|
accelUntil(-_aMax, 0, [pCen](Point pnt) { return pnt.p < pCen; });
|
|
accelUntil(_aMax, _wBack, [](Point pnt) { return pnt.w > 0; });
|
|
|
|
return transform();
|
|
}
|
|
|
|
Profile createJumpInTarget(float delta) {
|
|
init();
|
|
|
|
float jumpInRange = std::numbers::pi / 3;
|
|
float deltaPre = delta - jumpInRange;
|
|
|
|
smoothSubStep(deltaPre, _aMax, _wMax);
|
|
std::vector<float> jumpInSteps{jumpInRange / 2, jumpInRange / 3, jumpInRange / 6};
|
|
float nextPoint = deltaPre;
|
|
for (auto &&step : jumpInSteps) {
|
|
nextPoint = nextPoint + step;
|
|
smoothSubStep(nextPoint, _aMax * 10, _wMax * 2);
|
|
}
|
|
|
|
return transform();
|
|
}
|
|
|
|
private:
|
|
struct Point {
|
|
float p;
|
|
float w;
|
|
float t;
|
|
};
|
|
|
|
StepperMotor::Config &_config;
|
|
float _dT;
|
|
float _aMax;
|
|
float _wMax;
|
|
|
|
float _t;
|
|
std::vector<float> _pVec;
|
|
// std::vector<float> _wVec;
|
|
// std::vector<float> _tVec;
|
|
float _wBack;
|
|
float _tBack;
|
|
|
|
void init() {
|
|
_pVec = {0};
|
|
// _wVec = {0};
|
|
// _tVec = {0};
|
|
_wBack = 0;
|
|
_tBack = 0;
|
|
_t = 0;
|
|
}
|
|
|
|
void append(Point pnt) {
|
|
_pVec.push_back(pnt.p);
|
|
// _wVec.push_back(pnt.w);
|
|
// _tVec.push_back(pnt.t);
|
|
}
|
|
|
|
Point getStopDistance(float a, float w) {
|
|
float tDec = w / a;
|
|
float pDec = a * std::pow(tDec, 2) / 2;
|
|
return {pDec, w, tDec};
|
|
}
|
|
|
|
void accelUntil(float a, float wInit, std::function<bool(Point)> checkCbk) {
|
|
float t{};
|
|
auto pX = _pVec.back();
|
|
while (true) {
|
|
float w = wInit + a * t;
|
|
float p = pX + wInit * t + a * std::pow(t, 2) / 2;
|
|
if (checkCbk(Point{p, w, t})) {
|
|
break;
|
|
}
|
|
append(Point{p, w, _t + t});
|
|
t = t + _dT;
|
|
}
|
|
_t = _t + t;
|
|
}
|
|
|
|
void moveUntil(float wM, float pEnd) {
|
|
float t{};
|
|
auto pX = _pVec.back();
|
|
while (true) {
|
|
float p = pX + wM * t;
|
|
if (p > pEnd) {
|
|
break;
|
|
}
|
|
append(Point{p, wM, _t + t});
|
|
t = t + _dT;
|
|
}
|
|
_t = _t + t;
|
|
}
|
|
|
|
void buildUpSwing(float a, float vM, int swings) {
|
|
float t{};
|
|
auto pX = _pVec.back();
|
|
|
|
float TwMax = vM / a;
|
|
float tau = 2 * std::numbers::pi;
|
|
float sTau = swings * tau;
|
|
|
|
while (true) {
|
|
t = t + _dT;
|
|
float env = a * t;
|
|
float p = std::sin(sTau * t / TwMax) * env * TwMax / sTau;
|
|
float w = (p - _pVec.back()) / _dT;
|
|
if (w > _wMax) {
|
|
break;
|
|
}
|
|
append(Point{p, w, _t + t});
|
|
}
|
|
_t = _t + t;
|
|
}
|
|
|
|
void smoothSubStep(float delta, float aM, float wM) {
|
|
auto pnt = getStopDistance(aM, wM);
|
|
|
|
if (pnt.p < delta / 2) {
|
|
// t0..t1: acceleration to max velocity
|
|
accelUntil(aM, 0, [wM](Point pnt) { return pnt.w > wM; });
|
|
// t1..t2: movement to until almost destination
|
|
moveUntil(wM, delta - pnt.p);
|
|
// t2..r3: deceleration until destination
|
|
accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
|
|
} else {
|
|
float pCen = delta + (_pVec.back() - delta) / 2;
|
|
// t0..t1: acceleration until center is reached
|
|
accelUntil(-aM, 0, [pCen](Point pnt) { return pnt.p < pCen; });
|
|
// t2..r3: deceleration until destination
|
|
accelUntil(-aM, wM, [](Point pnt) { return pnt.w < 0; });
|
|
}
|
|
}
|
|
|
|
Profile transform() {
|
|
std::vector<uint8_t> steps;
|
|
steps.reserve(_pVec.size() + 1);
|
|
float stp = _config.stepRad * 4;
|
|
float res = stp / _config.resolution;
|
|
|
|
steps.push_back(0);
|
|
for (auto &&p : _pVec) {
|
|
auto f = p / stp;
|
|
f = f - std::floor(f);
|
|
uint8_t v = (uint8_t)(f * _config.resolution);
|
|
steps.push_back(v);
|
|
}
|
|
init(); // free memory for old values
|
|
return {steps};
|
|
}
|
|
};
|
|
|
|
void StepperMotor::createDemoProfile() {
|
|
|
|
long start = xTaskGetTickCount();
|
|
ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);
|
|
|
|
std::function<Profile(float)> call{};
|
|
static int idx = 0;
|
|
|
|
switch (idx) {
|
|
case 0: {
|
|
call = [&](float f) { return generator.createSmoothStep(f); };
|
|
} break;
|
|
case 1: {
|
|
call = [&](float f) { return generator.createBuildUpStep(f); };
|
|
} break;
|
|
case 2: {
|
|
call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
|
|
} break;
|
|
|
|
default:
|
|
case 3: {
|
|
call = [&](float f) { return generator.createJumpInTarget(f); };
|
|
} break;
|
|
}
|
|
|
|
idx++;
|
|
idx = idx % 4;
|
|
|
|
auto profile = call(std::numbers::pi);
|
|
long ticks = xTaskGetTickCount() - start;
|
|
|
|
ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
|
|
ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
|
|
|
|
auto pp = std::make_unique<Profile>(std::move(profile));
|
|
runProfile(std::move(pp));
|
|
}
|
|
|
|
void StepperMotor::moveBy(float angle) {
|
|
ProfileGenerator generator(_config, 0.001F, 1.0F, 1.0F);
|
|
auto profile = generator.createSmoothStep(angle);
|
|
|
|
ESP_LOGD("Stepper", "TookTicks for %d data points", profile.steps.size());
|
|
ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
|
|
|
|
auto pp = std::make_unique<Profile>(std::move(profile));
|
|
runProfile(std::move(pp));
|
|
}
|
|
|
|
void StepperMotor::moveBy(float angle, uint8_t idx, float accel, float speed) {
|
|
long start = xTaskGetTickCount();
|
|
ESP_LOGD("Stepper", "Running %.1f with prof=%i a=%.2f w=%.2f",
|
|
angle *3 /std::numbers::pi, idx, accel, speed);
|
|
|
|
ProfileGenerator generator(_config, 0.001F, accel, speed);
|
|
|
|
std::function<Profile(float)> call{};
|
|
switch (idx) {
|
|
case 0: {
|
|
call = [&](float f) { return generator.createSmoothStep(f); };
|
|
} break;
|
|
case 1: {
|
|
call = [&](float f) { return generator.createBuildUpStep(f); };
|
|
} break;
|
|
case 2: {
|
|
call = [&](float f) { return generator.createRapidStartSwingInEnd(f); };
|
|
} break;
|
|
|
|
default:
|
|
case 3: {
|
|
call = [&](float f) { return generator.createJumpInTarget(f); };
|
|
} break;
|
|
}
|
|
auto profile = call(angle);
|
|
// long ticks = xTaskGetTickCount() - start;
|
|
// ESP_LOGD("Stepper", "TookTicks %lu ( %lu ms) for %d data points", ticks, ticks * portTICK_PERIOD_MS, profile.steps.size());
|
|
// ESP_LOG_BUFFER_HEXDUMP("Stepper", profile.steps.data(), profile.steps.size(), ESP_LOG_DEBUG);
|
|
|
|
auto pp = std::make_unique<Profile>(std::move(profile));
|
|
runProfile(std::move(pp));
|
|
} |