motor control changed to infinit pipeline with continuous buffer
This commit is contained in:
@@ -20,7 +20,7 @@ class StepperDriverDrv8833 : public StepperDriver {
|
||||
std::array<gpio_num_t, 4> pins;
|
||||
};
|
||||
|
||||
StepperDriverDrv8833(Pins pins, int resolution);
|
||||
StepperDriverDrv8833(Pins pins, int resolution, float fullscale);
|
||||
StepCplx fullStep(int8_t cnt) override;
|
||||
StepCplx step(int8_t cnt) override;
|
||||
StepCplx setPos(uint8_t pos) override;
|
||||
@@ -39,4 +39,5 @@ class StepperDriverDrv8833 : public StepperDriver {
|
||||
std::vector<uint8_t> _outB1;
|
||||
std::vector<uint8_t> _outB2;
|
||||
void setOutput(uint8_t pos) override;
|
||||
const uint16_t FullScale;
|
||||
};
|
||||
@@ -0,0 +1,333 @@
|
||||
#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; });
|
||||
}
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
#include <cmath>
|
||||
#include <numbers>
|
||||
|
||||
struct StepperConfig {
|
||||
StepperConfig(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)};
|
||||
};
|
||||
@@ -7,17 +7,16 @@
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include "Semaphore.hpp"
|
||||
#include "gpTimer.hpp"
|
||||
#include "gpioPort.hpp"
|
||||
#include "profileGenerator.hpp"
|
||||
#include "stepperConfig.hpp"
|
||||
#include "stepperDrv.hpp"
|
||||
#include <Task.hpp>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
|
||||
struct Profile {
|
||||
std::vector<uint8_t> steps;
|
||||
};
|
||||
|
||||
class StepperMotor {
|
||||
enum Mode {
|
||||
Idle = 0,
|
||||
@@ -27,6 +26,7 @@ class StepperMotor {
|
||||
Homing,
|
||||
Run,
|
||||
CrawlToEnd,
|
||||
StartNew,
|
||||
};
|
||||
|
||||
public:
|
||||
@@ -100,27 +100,12 @@ class StepperMotor {
|
||||
}
|
||||
};
|
||||
|
||||
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)
|
||||
StepperMotor(StepperConfig &config, StepperDriver &driver, InputPort &initSensor)
|
||||
: _config(config), _driver(driver), _initSensor(initSensor),
|
||||
_timer(TimerCallParam, *this) {
|
||||
_timer(TimerCallParam, *this),
|
||||
_buffer{},
|
||||
_generator(_config, _buffer, 0.001F) {
|
||||
_timer.setup(1'000, HwTimerBase::Mode::Periodic);
|
||||
_timer.start();
|
||||
}
|
||||
@@ -128,17 +113,16 @@ class 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);
|
||||
}
|
||||
// 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);
|
||||
@@ -219,14 +203,9 @@ class StepperMotor {
|
||||
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;
|
||||
void newProfile() {
|
||||
_subStep = 0;
|
||||
_mode = StartNew;
|
||||
}
|
||||
|
||||
bool isIdle() {
|
||||
@@ -242,13 +221,16 @@ class StepperMotor {
|
||||
}
|
||||
|
||||
private:
|
||||
Config &_config;
|
||||
StepperConfig &_config;
|
||||
StepperDriver &_driver;
|
||||
InputPort &_initSensor;
|
||||
HwTimerT<StepperMotor &> _timer;
|
||||
Mode _mode{};
|
||||
int _step{};
|
||||
Trajectory _trajectory{};
|
||||
ProfileBuffer _buffer;
|
||||
ProfileGenerator _generator;
|
||||
int32_t _subStep;
|
||||
const ProfileBuffer::Segment *_pSlice{};
|
||||
|
||||
Position<50, 5> _indexPos{};
|
||||
|
||||
static bool TimerCallParam(HwTimerT<StepperMotor &> &, StepperMotor &stepper) {
|
||||
@@ -290,16 +272,28 @@ class StepperMotor {
|
||||
} break;
|
||||
|
||||
case Mode::Run: {
|
||||
auto stp = stepper._trajectory->steps[stepper._step];
|
||||
auto stp = stepper._pSlice->at(stepper._subStep);
|
||||
stepper._subStep++;
|
||||
if (stepper._subStep >= stepper._pSlice->size()) {
|
||||
stepper._subStep = 0;
|
||||
if (stepper._buffer.reachedFinal()) {
|
||||
stepper._mode = Idle;
|
||||
} else {
|
||||
stepper._pSlice = stepper._buffer.get();
|
||||
}
|
||||
}
|
||||
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::StartNew: {
|
||||
if (stepper._buffer.canStart()) {
|
||||
stepper._mode = Run;
|
||||
stepper._pSlice = stepper._buffer.get();
|
||||
stepper._subStep = 0;
|
||||
}
|
||||
}
|
||||
|
||||
case Mode::Idle:
|
||||
|
||||
default:
|
||||
|
||||
Reference in New Issue
Block a user