334 lines
9.0 KiB
C++
334 lines
9.0 KiB
C++
#pragma once
|
|
|
|
#include "esp_log.h"
|
|
#include "stepperConfig.hpp"
|
|
#include <Task.hpp>
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <vector>
|
|
|
|
class ProfileBuffer {
|
|
public:
|
|
static constexpr int SliceSize = 128;
|
|
static constexpr int BufferSize = 16;
|
|
using Segment = std::array<uint8_t, SliceSize>;
|
|
using Buffer = std::array<Segment, BufferSize>;
|
|
ProfileBuffer() : _semaphore(BufferSize) {}
|
|
|
|
void reset() {
|
|
_idxReader = 0;
|
|
_idxWriter = 0;
|
|
_written = 0;
|
|
_read = 0;
|
|
_minDiff = BufferSize;
|
|
_idxProcess = _scratchpad.size() - 1;
|
|
_writeFinished = false;
|
|
}
|
|
|
|
void append(Segment set, bool isFinal = false) {
|
|
// _semaphore.give();
|
|
_writeFinished = isFinal;
|
|
_scratchpad.at(_idxWriter) = set;
|
|
_idxWriter++;
|
|
if (_idxWriter >= _scratchpad.size()) {
|
|
_idxWriter = 0;
|
|
}
|
|
_written++;
|
|
_minDiff = std::min(_minDiff, _written - _read);
|
|
}
|
|
|
|
const Segment *get() {
|
|
// _semaphore.take();
|
|
_idxProcess = _idxReader;
|
|
_idxReader++;
|
|
if (_idxReader >= _scratchpad.size()) {
|
|
_idxReader = 0;
|
|
}
|
|
_read++;
|
|
_minDiff = std::min(_minDiff, _written - _read);
|
|
return &_scratchpad[_idxProcess];
|
|
}
|
|
|
|
bool reachedFinal() {
|
|
return _writeFinished && (_read == _written);
|
|
}
|
|
|
|
bool canWrite() {
|
|
return _idxWriter != _idxProcess;
|
|
}
|
|
|
|
bool canStart() {
|
|
return (_written > (BufferSize / 2));
|
|
}
|
|
|
|
uint32_t getStats() {
|
|
return _minDiff;
|
|
}
|
|
uint32_t getDiff() {
|
|
return _written - _read;
|
|
}
|
|
uint32_t getRead() {
|
|
return _read;
|
|
}
|
|
|
|
private:
|
|
Buffer _scratchpad{};
|
|
fos::Semaphore _semaphore;
|
|
uint32_t _idxReader{};
|
|
uint32_t _idxWriter{};
|
|
uint32_t _written{};
|
|
uint32_t _read{};
|
|
uint32_t _minDiff{BufferSize};
|
|
uint32_t _idxProcess = _scratchpad.size() - 1;
|
|
bool _writeFinished{};
|
|
};
|
|
|
|
class ProfileGenerator {
|
|
public:
|
|
enum Profile {
|
|
Smooth = 0,
|
|
BuildUp,
|
|
RapidStart,
|
|
JumpIn,
|
|
};
|
|
|
|
|
|
ProfileGenerator(StepperConfig &config, ProfileBuffer &buffer, float dT)
|
|
: _config(config), _transformer(config, buffer), _dT(dT), _aMax(1), _wMax(1) {
|
|
}
|
|
|
|
void setDynamic(float a, float w) {
|
|
_aMax = a;
|
|
_wMax = w;
|
|
}
|
|
|
|
void createProfile(Profile profile, float delta) {
|
|
init();
|
|
switch (profile) {
|
|
default:
|
|
case Profile::Smooth:
|
|
createSmoothStep(delta);
|
|
break;
|
|
case Profile::BuildUp:
|
|
createBuildUpStep(delta);
|
|
break;
|
|
case Profile::RapidStart:
|
|
createRapidStartSwingInEnd(delta);
|
|
break;
|
|
|
|
case Profile::JumpIn:
|
|
createJumpInTarget(delta);
|
|
break;
|
|
}
|
|
_transformer.finish();
|
|
}
|
|
|
|
private:
|
|
struct Point {
|
|
float p;
|
|
float w;
|
|
float t;
|
|
};
|
|
class Transformer {
|
|
public:
|
|
Transformer(StepperConfig &config, ProfileBuffer &profile)
|
|
: _config(config), _profile(profile) {
|
|
add(0);
|
|
}
|
|
|
|
void add(float p) {
|
|
auto f = p / stp;
|
|
f = f - std::floor(f);
|
|
_last = (uint8_t)(f * _config.resolution);
|
|
_temp[_idx] = _last;
|
|
_idx++;
|
|
if (_idx >= _temp.size()) {
|
|
awaitCanWrite();
|
|
_profile.append(_temp);
|
|
ESP_LOGD("Profile", "add slice, canStart %d", _profile.canStart());
|
|
_idx = 0;
|
|
}
|
|
}
|
|
void finish() {
|
|
while (_idx < _temp.size()) {
|
|
_temp[_idx] = _last;
|
|
_idx++;
|
|
}
|
|
awaitCanWrite();
|
|
_profile.append(_temp, true);
|
|
ESP_LOGD("Profile", "add final slice");
|
|
_idx = 0;
|
|
}
|
|
|
|
void awaitCanWrite() {
|
|
while (!_profile.canWrite()) {
|
|
fos::Task::delayMs(200);
|
|
ESP_LOGD("Profile", "waiting... diff=%lu read=%lu canStart %d",
|
|
_profile.getDiff(), _profile.getRead(), _profile.canStart());
|
|
}
|
|
}
|
|
|
|
private:
|
|
StepperConfig &_config;
|
|
ProfileBuffer &_profile;
|
|
const float stp = _config.stepRad * 4;
|
|
const float res = stp / _config.resolution;
|
|
ProfileBuffer::Segment _temp{};
|
|
uint32_t _idx{};
|
|
uint8_t _last;
|
|
};
|
|
|
|
StepperConfig &_config;
|
|
Transformer _transformer;
|
|
float _dT;
|
|
float _aMax;
|
|
float _wMax;
|
|
|
|
float _t;
|
|
// std::vector<float> _pVec;
|
|
// std::vector<float> _wVec;
|
|
// std::vector<float> _tVec;
|
|
float _pBack;
|
|
float _wBack;
|
|
float _tBack;
|
|
|
|
void createSmoothStep(float delta) {
|
|
// already solved in simple step
|
|
smoothSubStep(delta, _aMax, _wMax);
|
|
}
|
|
|
|
void createBuildUpStep(float delta) {
|
|
// 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; });
|
|
}
|
|
|
|
void createRapidStartSwingInEnd(float delta) {
|
|
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 + (_pBack - delta) / 2;
|
|
accelUntil(-_aMax, 0, [pCen](Point pnt) { return pnt.p < pCen; });
|
|
accelUntil(_aMax, _wBack, [](Point pnt) { return pnt.w > 0; });
|
|
}
|
|
|
|
void createJumpInTarget(float delta) {
|
|
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);
|
|
}
|
|
}
|
|
|
|
void init() {
|
|
_pBack = 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);
|
|
_pBack = pnt.p;
|
|
_wBack = pnt.w;
|
|
_transformer.add(pnt.p);
|
|
}
|
|
|
|
void finish() {
|
|
}
|
|
|
|
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 = _pBack;
|
|
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 = _pBack;
|
|
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 = _pBack;
|
|
|
|
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 - _pBack) / _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 + (_pBack - 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; });
|
|
}
|
|
}
|
|
};
|