exodus/1NiM-WatingRoom-WatchController
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

motor control changed to infinit pipeline with continuous buffer

Browse Source
This commit is contained in:
Jakob Gegeniger
2024-12-22 17:01:06 +01:00
parent eb0eb436ed
commit dd9f59e418
8 changed files with 593 additions and 593 deletions
Download Patch File Download Diff File Expand all files Collapse all files
components/movement/include/StepperDrv8833.hpp
+2 -1
View File
@@ -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;
};
components/movement/include/profileGenerator.hpp
+333
View File
@@ -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; });
}
}
};
components/movement/include/stepperConfig.hpp
+19
View File
@@ -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)};
};
components/movement/include/stepperMotor.hpp
+45 -51
View File
@@ -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: