motor control changed to infinit pipeline with continuous buffer

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: