motor control changed to infinit pipeline with continuous buffer

2024-12-22 17:01:06 +01:00
parent eb0eb436ed
commit dd9f59e418
8 changed files with 593 additions and 593 deletions
@@ -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; });
+        }
+    }
+};