motor control changed to infinit pipeline with continuous buffer

components/movement/include/StepperDrv8833.hpp

@@ -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

@@ -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

@@ -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

@@ -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:

components/movement/src/stepperDrv8833.cpp

@@ -14,9 +14,11 @@
 
 void populateLookUps(int resolution,
                      std::vector<uint8_t> &a1, std::vector<uint8_t> &a2,
-                     std::vector<uint8_t> &b1, std::vector<uint8_t> &b2) {
+                     std::vector<uint8_t> &b1, std::vector<uint8_t> &b2, 
+                     uint16_t fullscale) 
+                      {
     float step = 2.0F * std::numbers::pi / resolution;
-    const uint16_t FullScale = 1 << LEDC_TIMER_7_BIT;
+    const uint16_t Maximum = 1 << LEDC_TIMER_7_BIT;
     
     for (size_t i = 0; i < resolution; i++) {
         
@@ -25,13 +27,13 @@ void populateLookUps(int resolution,
         {
             // 1 :   0 = Forward
             // 1 : PWM = Forward PWM, slow decay
-            a1.push_back(FullScale);
-            a2.push_back(FullScale - (cos * FullScale));
+            a1.push_back(Maximum);
+            a2.push_back(Maximum - (cos * fullscale));
         } else {
             //   0 : 1 = Reverse
             // PWM : 1 = Reverse PWM, slow decay
-            a1.push_back(FullScale - (-cos * FullScale));
-            a2.push_back(FullScale);
+            a1.push_back(Maximum - (-cos * fullscale));
+            a2.push_back(Maximum);
         }
 
         auto sin = std::sin(step * i);
@@ -39,29 +41,30 @@ void populateLookUps(int resolution,
         {
             // 1 :   0 = Forward
             // 1 : PWM = Forward PWM, slow decay
-            b1.push_back(FullScale);
-            b2.push_back(FullScale - (sin * FullScale));
+            b1.push_back(Maximum);
+            b2.push_back(Maximum - (sin * fullscale));
         } else {
             //   0 : 1 = Reverse
             // PWM : 1 = Reverse PWM, slow decay
-            b1.push_back(FullScale - ((-sin * FullScale)));
-            b2.push_back(FullScale);
+            b1.push_back(Maximum - ((-sin * fullscale)));
+            b2.push_back(Maximum);
         }
     };
 }
 
-StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution)
+StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution, float fullscale)
     : StepperDriver(resolution), 
       _pwmA1(pins.timer, pins.channels[0], pins.pins[0], LEDC_TIMER_7_BIT, 20000),
       _pwmA2(pins.timer, pins.channels[1], pins.pins[1], LEDC_TIMER_7_BIT, 20000),
       _pwmB1(pins.timer, pins.channels[2], pins.pins[2], LEDC_TIMER_7_BIT, 20000),
-      _pwmB2(pins.timer, pins.channels[3], pins.pins[3], LEDC_TIMER_7_BIT, 20000)
+      _pwmB2(pins.timer, pins.channels[3], pins.pins[3], LEDC_TIMER_7_BIT, 20000),
+      FullScale((1 << LEDC_TIMER_7_BIT) * std::max(0.0F, std::min(1.0F, fullscale)))
       {
     if ((Resolution % 4) != 0) {
         ESP_LOGE("Stepper", "Resolution must be multiple of 4");
     }
 
-    populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2);
+    populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2, FullScale);
 
     ESP_LOGD("Stepper", "Output Data:");
     ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA1.data(), _outA1.size(), ESP_LOG_DEBUG);

components/movement/src/stepperMotor.cpp

@@ -12,276 +12,31 @@
 #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 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 - pX) / _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));
+    _buffer.reset();
+    newProfile();
+    _generator.createProfile(ProfileGenerator::Profile::Smooth, 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);
 }
 
 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);
+    // long start = xTaskGetTickCount();
+    ESP_LOGI("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);
+    _buffer.reset();
+    newProfile();
+    _generator.setDynamic(accel, speed);
+    _generator.createProfile(static_cast<ProfileGenerator::Profile>(idx), angle);
 
-    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;
+    while (!isIdle())
+    {
+        fos::Task::delayMs(200);
     }
-    auto profile = call(angle);
+    ESP_LOGI("Profile", "processing finished ...");
+
     // 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));
 }