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 {
+    StepperMotor(StepperConfig &config, StepperDriver &driver, InputPort &initSensor)
-        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)
         : _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()) {
+        // if (_initSensor.read()) {
-            _mode = Mode::HomingFromSensor;
+        //     _mode = Mode::HomingFromSensor;
-            ESP_LOGI("Stepper", "First leave HomePos");
+        //     ESP_LOGI("Stepper", "First leave HomePos");
-        } else {
+        // } else {
-            _mode = Mode::HomingToSensorBegin;
+        //     _mode = Mode::HomingToSensorBegin;
-            ESP_LOGI("Stepper", "Find start of HomePos");
+        //     ESP_LOGI("Stepper", "Find start of HomePos");
-        }
+        // }
-
+        // while (_mode != Mode::Idle) {
-        while (_mode != Mode::Idle) {
+        //     fos::Task::delayTicks(1);
-            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) {
+    void newProfile() {
-        if (profile == nullptr) {
+        _subStep = 0;
-            return;
+        _mode = StartNew;
-        }
-
-        _trajectory = std::move(profile);
-        _step = 0;
-        _mode = Run;
     }
 
     bool isIdle() {
@@ -242,13 +221,16 @@ class StepperMotor {
     }
 
   private:
-    Config &_config;
+    StepperConfig &_config;
     StepperDriver &_driver;
     InputPort &_initSensor;
     HwTimerT<StepperMotor &> _timer;
     Mode _mode{};
-    int _step{};
+    ProfileBuffer _buffer;
-    Trajectory _trajectory{};
+    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);
+            a1.push_back(Maximum);
-            a2.push_back(FullScale - (cos * FullScale));
+            a2.push_back(Maximum - (cos * fullscale));
         } else {
             //   0 : 1 = Reverse
             // PWM : 1 = Reverse PWM, slow decay
-            a1.push_back(FullScale - (-cos * FullScale));
+            a1.push_back(Maximum - (-cos * fullscale));
-            a2.push_back(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);
+            b1.push_back(Maximum);
-            b2.push_back(FullScale - (sin * FullScale));
+            b2.push_back(Maximum - (sin * fullscale));
         } else {
             //   0 : 1 = Reverse
             // PWM : 1 = Reverse PWM, slow decay
-            b1.push_back(FullScale - ((-sin * FullScale)));
+            b1.push_back(Maximum - ((-sin * fullscale)));
-            b2.push_back(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);
+    _buffer.reset();
-    auto profile = generator.createSmoothStep(angle);
+    newProfile();
-
+    _generator.createProfile(ProfileGenerator::Profile::Smooth, angle);
-    ESP_LOGD("Stepper", "TookTicks for %d data points", profile.steps.size());
+    // 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);
+    // 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, uint8_t idx, float accel, float speed) {
-    long start = xTaskGetTickCount();
+    // long start = xTaskGetTickCount();
-    ESP_LOGD("Stepper", "Running %.1f with prof=%i a=%.2f w=%.2f", 
+    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{};
+    while (!isIdle())
-    switch (idx) {
+    {
-    case 0: {
+        fos::Task::delayMs(200);
-        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;
     }
-    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));
 }

main/watchMain.cpp

@@ -8,10 +8,6 @@
 
 #include "watchMain.hpp"
 
-static const char *TAG = "Watch";
-static esp_eth_handle_t s_eth_handle = NULL;
-static bool s_ethernet_is_connected = false;
-
 extern "C" { // This switch allows the ROS C-implementation to find this main
 void app_main(void);
 }
@@ -25,11 +21,9 @@ struct DeviceState {
     Color color{16, 16, 16};
 };
 
-DeviceState activeState;
 enum class MoveType {
     Step,
-    AllPictograms,
+    Demo
-    Arbitrary,
 };
 struct MoveCommand {
     MoveType type;
@@ -39,12 +33,18 @@ struct MoveCommand {
 };
 static fos::Queue<MoveCommand> commandQueue(8);
 
+DeviceState activeState;
+static const char *TAG = "Watch";
+static bool EthernetIsConnected = false;
 
 std::unique_ptr<Cli> shellPtr;
 
-/// @brief Callback for update start
+void onCommand(std::string name, Cli::CArgs args) {
-/// @param args callback arguments
+    ESP_LOGI(TAG, "Invoking: %s", name.c_str());
+}
+
 void startUpdate(Cli::CArgs args) {
+
     ESP_LOGI(TAG, "Invoking Update");
     if (args.size() != 1) {
         shellPtr->write("Need URL to application'\n");
@@ -53,24 +53,13 @@ void startUpdate(Cli::CArgs args) {
     // @ToDo: Start Update here
 }
 
-/// @brief Callback for commands that are not directly wired
-/// @param name command name
-/// @param args callback arguments
-void onCommand(std::string name, Cli::CArgs args) {
-    ESP_LOGI(TAG, "Invoking: %s", name.c_str());
-}
-
-void showLogServerConfig(Cli::CArgs args) {
-    ESP_LOGI(TAG, "on showLogServerConfig:");
-    // auto &store = NvsApi::getInstance();
-    // auto logger = store.getValue("syslogDst");
-    // shellPtr->write(std::string{"syslog destination = '"} + logger + "'\n");
-}
-
 void setLogServerConfig(Cli::CArgs args) {
     ESP_LOGI(TAG, "on showLogServerConfig:");
     if (args.size() != 1) {
         shellPtr->write("Need IP address to syslog server'\n");
+        // auto &store = NvsApi::getInstance();
+        // auto logger = store.getValue("syslogDst");
+        // shellPtr->write(std::string{"syslog destination = '"} + logger + "'\n");
         return;
     }
     // auto &store = NvsApi::getInstance();
@@ -78,6 +67,46 @@ void setLogServerConfig(Cli::CArgs args) {
     // store.setValue("syslogDst", args[0]);
 }
 
+void setDynamics(Cli::CArgs args) {
+    ESP_LOGI(TAG, "on setDynamics:");
+    if (args.size() != 2) {
+        shellPtr->write("need float values for velocity and acceleration'\n");
+        return;
+    }
+
+    try {
+        float v = std::stof(args[0]);
+        float a = std::stof(args[1]);
+        activeState.accel = a;
+        activeState.speed = v;
+    } catch (const std::exception &e) {
+        ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
+    }
+}
+
+void triggerMovement(Cli::CArgs args) {
+    ESP_LOGI(TAG, "on triggerMovement:");
+    if (args.size() != 2) {
+        shellPtr->write("need profile index and steps'\n");
+        return;
+    }
+
+    try {
+        int p = std::stoi(args[0]);
+        int j = std::stoi(args[1]);
+        activeState.profile = p;
+        activeState.jumps = j;
+        commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Step, j, p, activeState.color));
+    } catch (const std::exception &e) {
+        ESP_LOGI(TAG, "Failed to parse %s and %s", args[0].c_str(), args[1].c_str());
+    }
+}
+
+void startDemoMovement(Cli::CArgs args) {
+    ESP_LOGI(TAG, "on startDemoMovement:");
+    commandQueue.enqueue(std::make_unique<MoveCommand>(MoveType::Demo));
+}
+
 /**
  * @brief Telnet socket via TCP/IP
  *
@@ -94,12 +123,15 @@ static void telnetSocketTask(int addrFamily) {
                 shellPtr->addBinding(
                     {"update", "Start OTA-Update-Process",
                      [](Cli::CArgs a) { startUpdate(a); }});
-                shellPtr->addBinding(
-                    {"logserver_show", "Shows log server config ip",
-                     [](Cli::CArgs a) { showLogServerConfig(a); }});
                 shellPtr->addBinding(
                     {"logserver_set", "Set log server config ip",
                      [](Cli::CArgs a) { setLogServerConfig(a); }});
+                shellPtr->addBinding(
+                    {"dyn_set", "set dynamics of movement (velocity,accelerations) as floats",
+                     [](Cli::CArgs a) { setDynamics(a); }});
+                shellPtr->addBinding(
+                    {"move", "start movement (profile-idx,steps) as integers ",
+                     [](Cli::CArgs a) { triggerMovement(a); }});
             },
             [](TcpSession &session) {
                 shellPtr->process();
@@ -108,11 +140,9 @@ static void telnetSocketTask(int addrFamily) {
     } catch (const std::exception &e) {
         ESP_LOGE("TCP", "%s", e.what());
     }
-
     vTaskDelete(NULL);
 }
 
-
 constexpr uint32_t DmxUniverse = 0;
 constexpr int DmxChannelCount = 9;
 
@@ -134,11 +164,11 @@ static void UniverseCallback(const uint8_t *data, const uint16_t size) {
                  data[0], data[1], data[2], data[3], data[4], data[5], data[6], data[7], data[8]);
 
     float intens = (float)data[3] / UINT8_MAX;
-    // activeState.color = Color{
+    activeState.color = Color{
-    //     scaledRgbChannel(data[0], intens),
+        scaledRgbChannel(data[0], intens),
-    //     scaledRgbChannel(data[1], intens),
+        scaledRgbChannel(data[1], intens),
-    //     scaledRgbChannel(data[2], intens),
+        scaledRgbChannel(data[2], intens),
-    //     0};
+        0};
 
     static uint8_t lastTrig = 0;
     uint8_t thisTrig = data[4];
@@ -225,20 +255,11 @@ void startServerTasks() {
     static fos::TaskT<int> telnetSocket(telnetSocketTask, ipVersion, "tcpServer", 4096);
     static fos::TaskT<int> dmxNode(dmxSocketTask, ipVersion, "dmxNode", 4096);
     // static Task otaUpdate(SystemFunctions::OtaTaskEntry, "otaTask", 8192);
-    // initialise_mdns();
-    // netbiosns_init();
-    // netbiosns_set_name(CONFIG_EXAMPLE_MDNS_HOST_NAME);
-    // auto &store = NvsApi::getInstance();
-    // auto logger = store.getValue("syslogDst");
-    // if (logger != "") {
-    //     Syslog::setupTask(IpAdr{logger, 0});
-    // }
 }
 
 /** Event handler for IP_EVENT_ETH_GOT_IP */
 static void got_ip_event_handler(void *arg, esp_event_base_t event_base,
-                                 int32_t event_id, void *event_data)
+                                 int32_t event_id, void *event_data) {
-                                 {
     ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
     const esp_netif_ip_info_t *ip_info = &event->ip_info;
 
@@ -253,20 +274,19 @@ static void got_ip_event_handler(void *arg, esp_event_base_t event_base,
 
 // Event handler for Ethernet
 static void eth_event_handler(void *arg, esp_event_base_t event_base,
-                              int32_t event_id, void *event_data)
+                              int32_t event_id, void *event_data) {
-                              {
     uint8_t mac_addr[6] = {0};
     switch (event_id) {
     case ETHERNET_EVENT_CONNECTED:
         ESP_LOGI(TAG, "Ethernet Link Up");
-        s_ethernet_is_connected = true;
+        EthernetIsConnected = true;
         ESP_LOGI(TAG, "Ethernet Link Up");
         ESP_LOGI(TAG, "Ethernet HW Addr %02x:%02x:%02x:%02x:%02x:%02x",
                  mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
         break;
     case ETHERNET_EVENT_DISCONNECTED:
         ESP_LOGI(TAG, "Ethernet Link Down");
-        s_ethernet_is_connected = false;
+        EthernetIsConnected = false;
         break;
     case ETHERNET_EVENT_START:
         ESP_LOGI(TAG, "Ethernet Started");
@@ -279,8 +299,7 @@ static void eth_event_handler(void *arg, esp_event_base_t event_base,
     }
 }
 
-static void initialize_ethernet(void) 
+static void initialize_ethernet(void) {
-{
     uint8_t eth_port_cnt = 0;
     esp_eth_handle_t *eth_handles;
     ESP_ERROR_CHECK(example_eth_init(&eth_handles, &eth_port_cnt));
@@ -288,6 +307,7 @@ static void initialize_ethernet(void)
         ESP_LOGW(TAG, "multiple Ethernet devices detected, the first initialized is to be used!");
     }
 
+    static esp_eth_handle_t s_eth_handle = NULL;
     s_eth_handle = eth_handles[0];
     free(eth_handles);
 
@@ -343,91 +363,53 @@ void LightingTask() {
     static PmwExpanderPca9685 ledDriver2(master, /*iverted=*/false, 0x41);
     PmwExpanderPca9685::GrayValues values[2];
 
-    OutputPort pwmEnable(GPIO_NUM_5, GpioPort::Logic::Inverted);
+    OutputPort pwmEnable(GPIO_NUM_1, GpioPort::Logic::Inverted);
     pwmEnable.write(true);
 
-    int idx = 0;
+    auto const &colorSource = activeState.color;
-    while (true) {
+    std::array<Effect::EffectProcessor, 6> processors{
-        values[0].fill(0);
+        Effect::EffectProcessor(8),
-        values[1].fill(0);
+        Effect::EffectProcessor(8),
-
+        Effect::EffectProcessor(8),
-        // values[x][4] = 0x200; // Actually first
+        Effect::EffectProcessor(8),
-        values[0][idx] = 0x200;
+        Effect::EffectProcessor(8),
-        ledDriver1.sendImage(values[0]);
+        Effect::EffectProcessor(8)};
-
+    processors[0].setEffect(Effect::macDemoStart, &colorSource, 16);
-        // values[1][4] = 0x100; // Actually first
+    processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
-        values[1][idx] = 0x400;
+    processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
-        ledDriver2.sendImage(values[1]);
+    processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
-
+    processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
-        if (++idx >= 16 ) {
+    processors[5].setEffect(Effect::macStartIdle, &colorSource, 96);
-            idx = 0;
-        }
-        fos::Task::delayMs(100);
-    }
-
-
-    // ESP_LOGI(TAG, "Start Lighting");
-
-    // static I2cMaster master(I2C_NUM_0, GPIO_NUM_10, GPIO_NUM_11);
-    // static PmwExpanderPca9685 pca9685(master);
-    // PmwExpanderPca9685::GrayValues values;
-
-    // AddressableLeRtz ledBar(GPIO_NUM_15, AddressableLeRtz::Type::Rgb, 6);
-    // std::vector<Color> pictoSpots(6);
-    // for (auto &&led : pictoSpots) {
-    //     led.set(0, 0, 0);
-    // }
-
-    // auto const &colorSource = activeState.color;
-    // std::array<Effect::EffectProcessor, 6> processors{
-    //     Effect::EffectProcessor(8),
-    //     Effect::EffectProcessor(8),
-    //     Effect::EffectProcessor(8),
-    //     Effect::EffectProcessor(8),
-    //     Effect::EffectProcessor(8),
-    //     Effect::EffectProcessor(8)};
-
-    // processors[0].setEffect(Effect::macDemoStart, &colorSource, 16);
-    // processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
-    // processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
-    // processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
-    // processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
-    // processors[5].setEffect(Effect::macStartIdle, &colorSource, 96);
-
-    // int idx = 0;
-    // values.fill(0);
-    // while (true) {
-    //     values[0] = activeState.color.getRed() * 257;
-    //     values[1] = activeState.color.getGreen() * 257;
-    //     values[2] = activeState.color.getBlue() * 257;
-    //     pca9685.sendImage(values);
-
-    //     for (size_t i = 0; i < 600; i++) {
-    //         Task::delayMs(20);
-    //         for (size_t i = 0; i < pictoSpots.size(); i++) {
-    //             pictoSpots[i] = processors[i].tick();
-    //         }
-    //         ledBar.send(pictoSpots);
-    //     }
 
     //     processors[1].setEffect(Effect::macDark, &colorSource, 32);
     //     processors[2].setEffect(Effect::macDark, &colorSource, 48);
     //     processors[3].setEffect(Effect::macDark, &colorSource, 64);
     //     processors[4].setEffect(Effect::macDark, &colorSource, 80);
-
-    //     for (size_t i = 0; i < 50; i++) {
-    //         Task::delayMs(20);
-    //         for (size_t i = 0; i < pictoSpots.size(); i++) {
-    //             pictoSpots[i] = processors[i].tick();
-    //         }
-    //         ledBar.send(pictoSpots);
-    //     }
-
     //     processors[1].setEffect(Effect::macDemoStart, &colorSource, 32);
     //     processors[2].setEffect(Effect::macDemoPulse, &colorSource, 48);
     //     processors[3].setEffect(Effect::macDemoPulse, &colorSource, 64);
     //     processors[4].setEffect(Effect::macStartFlicker, &colorSource, 80);
-    // }
+
+
+    while (true) {
+        values[0].fill(0);
+        values[1].fill(0);
+
+        // values[x][4] = 0x200; // Actually first
+        values[0][4] = activeState.color.getRed() * 257;
+        values[0][5] = activeState.color.getGreen() * 257;
+        values[0][6] = activeState.color.getBlue() * 257;
+        
+        // values[1][4] = 0x100; // Actually first
+        values[1][4] = activeState.color.getRed() * 257;
+        values[1][5] = activeState.color.getGreen() * 257;
+        values[1][6] = activeState.color.getBlue() * 257;
+        
+        ledDriver1.sendImage(values[0]);
+        ledDriver2.sendImage(values[1]);
+        fos::Task::delayMs(40);
+    }
+
 }
 
 void MovementTask() {
@@ -438,10 +420,10 @@ void MovementTask() {
         {LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
         {GPIO_NUM_23, GPIO_NUM_22, GPIO_NUM_21, GPIO_NUM_20},
     };
-    StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps);
+    StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps, 0.25F);
     stepperDriver.setPos(0);
 
-    StepperMotor::Config stepperCfg{1.8F, MicroSteps};
+    StepperConfig stepperCfg{1.8F, MicroSteps};
     InputPort initSens(GPIO_NUM_18);
 
     InputPort motorFault(GPIO_NUM_8);
@@ -452,127 +434,56 @@ void MovementTask() {
 
     StepperMotor stepper(stepperCfg, stepperDriver, initSens);
 
-    int i = 0;
-    while (i++ < 80) {
-        stepperDriver.step(1);
-        stepperDriver.getSubPos();
-        fos::Task::delayMs(100);
-    }
-
     ESP_LOGI("Stepper", "Find Home");
     stepper.findRef();
     ESP_LOGI(TAG, "Fist Position is %f.2", stepper.getScaledPos() * 6);
     fos::Task::delayMs(1000);
 
     while (true) {
+        ESP_LOGD(TAG, "Waiting for command");
+        auto cmd = commandQueue.dequeue();
+        if (cmd->type == MoveType::Step) {
+            if (stepper.isIdle()) {
+                auto delta = cmd->deltaPic;
+                auto profile = cmd->profile;
+
+                // activeState.pictogram = ledPos;
+                // ledRing.send(ledStrip);
+                delta = std::min<uint8_t>(delta, 128);
+                Syslog::logv(Syslog::Severity::Notice,
+                             "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f",
+                             delta, profile, activeState.accel, activeState.speed);
+                stepper.moveBy(delta * std::numbers::pi / 3,
+                               profile, activeState.accel, activeState.speed);
+            } else {
                 fos::Task::delayMs(100);
             }
-
+        } else if (cmd->type == MoveType::Demo) {
-
+            int i = 0;
-
+            while (i++ < 80) {
-    // ESP_LOGI(TAG, "Start Stepper");
+                stepperDriver.step(1);
-    // StepperDriverTb6612::Pins stepperPins{
+                stepperDriver.getSubPos();
-    //     LEDC_TIMER_0,
+                fos::Task::delayMs(100);
-    //     LEDC_CHANNEL_0,
+            }
-    //     GPIO_NUM_23,
+            ESP_LOGI("Stepper", "Find Home");
-    //     {GPIO_NUM_22, GPIO_NUM_21},
+            stepper.findRef();
-    //     LEDC_CHANNEL_1,
+            activeState.pictogram = -1;
-    //     GPIO_NUM_20,
+            stepper.findRef();
-    //     {GPIO_NUM_19, GPIO_NUM_18},
+            fos::Task::delayMs(1000);
-    // };
+            for (size_t i = 0; i < 6; i++) {
-    // constexpr int MicroSteps = 32;
+                stepper.moveBy(std::numbers::pi / 3);
-    // StepperDriverTb6612 stepperDriver(stepperPins, MicroSteps);
+                while (!stepper.isIdle()) {
-    // stepperDriver.setPos(0);
+                    fos::Task::delayMs(100);
-
+                }
-    // StepperMotor::Config stepperCfg{1.8F, MicroSteps};
+                ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
-    // InputPort initSens(GPIO_NUM_3);
+            }
-    // initSens.setLogic(GpioPort::Logic::Inverted);
+        } else {
-    // StepperMotor stepper(stepperCfg, stepperDriver, initSens);
+            fos::Task::delayMs(100);
-
+        }
-    // ESP_LOGI(TAG, "Find Home");
+    }
-
-    // stepper.findRef();
-    // ESP_LOGI(TAG, "Fist Position is %f.2", stepper.getScaledPos() * 6);
-    // Task::delayMs(1000);
-
-    // AddressableLeRtz ledRing(GPIO_NUM_9, AddressableLeRtz::Type::Rgb, 12);
-    // std::vector<Color> ledStrip(12);
-    // for (auto &&led : ledStrip) {
-    //     led.set(0, 0, 0);
-    // }
-
-    // uint32_t ledPos = 0;
-    // ledStrip[0].set(
-    //     activeState.color.getRed(),
-    //     activeState.color.getGreen(),
-    //     activeState.color.getBlue(),
-    //     0);
-    // ledRing.send(ledStrip);
-
-    // while (true) {
-    //     ESP_LOGD(TAG, "Waiting for command");
-    //     auto cmd = commandQueue.dequeue();
-    //     if (cmd->type == MoveType::Step) {
-    //         if (stepper.isIdle()) {
-    //             auto delta = cmd->deltaPic;
-    //             auto profile = cmd->profile;
-
-    //             for (auto &&led : ledStrip) {
-    //                 led.set(0, 0, 0);
-    //             }
-    //             ledPos = (ledPos + delta) % 6;
-    //             activeState.pictogram = ledPos;
-
-    //             ledStrip[ledPos * 2].set(
-    //                 cmd->color.getRed(),
-    //                 cmd->color.getGreen(),
-    //                 cmd->color.getBlue(),
-    //                 0);
-    //             ledRing.send(ledStrip);
-    //             MqttClient::publishPosition(ledPos);
-
-    //             delta = std::min<uint8_t>(delta, 7);
-    //             Syslog::logv(Syslog::Severity::Notice, "Running Stepper by %i steps with prof=%i a=%.2f w=%.2f", delta, profile, activeState.accel, activeState.speed);
-    //             stepper.moveBy(delta * std::numbers::pi / 3,
-    //                            profile, activeState.accel, activeState.speed);
-    //         }
-    //     } else if (cmd->type == MoveType::AllPictograms) {
-    //         activeState.pictogram = -1;
-    //         stepper.findRef();
-    //         Task::delayMs(1000);
-    //         for (size_t i = 0; i < 6; i++) {
-    //             for (auto &&led : ledStrip) {
-    //                 led.set(0, 0, 0);
-    //             }
-    //             ledStrip[i * 2].set(0, 0, 16);
-    //             ledRing.send(ledStrip);
-    //             stepper.moveBy(std::numbers::pi / 3);
-    //             while (!stepper.isIdle()) {
-    //                 Task::delayMs(100);
-    //             }
-    //             ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
-    //         }
-    //     } else if (cmd->type == MoveType::Arbitrary) {
-    //         activeState.pictogram = -1;
-    //         ESP_LOGI(TAG, "Full Turn");
-    //         stepper.demoFullTurn();
-    //         Task::delayMs(4000);
-
-    //         while (true) {
-    //             if (stepper.isIdle()) {
-    //                 ESP_LOGI(TAG, "Position is %f.2", stepper.getScaledPos() * 6);
-    //                 Task::delayMs(500);
-    //                 stepper.createDemoProfile();
-    //             }
-    //             Task::delayMs(20);
-    //         }
-    //     }
-    // }
-
 }
 
-void IoTest() 
+void IoTest() {
-{
     ESP_LOGI(TAG, "Start IO");
     InputPort in2(GPIO_NUM_19);
     OutputPort nReset(GPIO_NUM_9);
@@ -583,8 +494,7 @@ void IoTest()
     }
 }
 
-void app_main(void) 
+void app_main(void) {
-{
     esp_err_t ret = nvs_flash_init();
     if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
         ESP_ERROR_CHECK(nvs_flash_erase());
@@ -592,7 +502,7 @@ void app_main(void)
     }
     ESP_ERROR_CHECK(ret);
     initialize_ethernet();
-    static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
+    // static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
     static fos::Task movementTask(MovementTask, "Motor", 0x4000uL);
-    static fos::Task ioTask(IoTest, "IO", 0x4000uL);
+    // static fos::Task ioTask(IoTest, "IO", 0x4000uL);
 }

main/watchMain.hpp

@@ -10,9 +10,6 @@
 #include <string.h>
 #include <stdlib.h>
 #include "sdkconfig.h"
-#include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-#include "freertos/queue.h"
 #include "esp_event.h"
 #include "esp_log.h"
 #include "esp_eth_driver.h"
@@ -55,29 +52,17 @@
 // #include "freertos/task.h"
 // #include "gpTimer.hpp"
 // #include "gpioPort.hpp"
-// #include "i2cPort.hpp"
 // #include "lwip/apps/netbiosns.h"
 // #include "mdns.h"
 // #include "mqttClient.hpp"
 // #include "nlohmann/json.hpp"
 // #include "nvsFunctions.hpp"
 // #include "pwmExpanderPca9685.hpp"
-// #include "pwmPort.hpp"
 // #include "rmtPort.hpp"
-// #include "sdkconfig.h"
 // #include "soc/soc_caps.h"
-// #include "stepperMotor.hpp"
-// #include "stepperTB6612.hpp"
-// #include "stepperDrv8833.hpp"
-// #include "string.h"
 // #include "systemFunctions.hpp"
-// #include "tcpSockets.hpp"
-// #include "uartPort.hpp"
-// #include "udpSockets.hpp"
-// #include "webServer.hpp"
-// #include "wifiFunctions.hpp"
 // #include <inttypes.h>
-// #include <string.h>
+// #include <cstring>
 // #include <sys/param.h>
 // #include <unistd.h>
 // #include <cmath>