PWM based motor driver added from other project

components/board_io/CMakeLists.txt

@@ -0,0 +1,10 @@
+idf_component_register(
+    SRC_DIRS "src"
+    INCLUDE_DIRS "include"
+    REQUIRES freeRtosApi 
+        project_bases
+        esp_common 
+        esp_driver_gpio 
+        esp_driver_ledc
+        esp_driver_gptimer
+)

components/board_io/include/gpioPort.hpp

@@ -0,0 +1,95 @@
+/**
+ * @file gpioPort.hpp
+ * @author Gustice
+ * @brief GPIO-Port classes
+ * @details C++ Wrapper for GPIO port
+ *
+ * @copyright Copyright (c) 2024
+ */
+#pragma once
+
+#include "driver/gpio.h"
+#include "portBase.hpp"
+
+/**
+ * @brief Digital-Port BaseClass
+ */
+class GpioPort : public PortBase {
+  public:
+    enum PullResistor {
+        Floating = 0,
+        PullUp,
+        PullDown,
+    };
+
+    enum Logic {
+        Normal = 0, // Normal logic
+        Inverted,   // Inverted logic -> Set low on active state
+    };
+
+    enum Interrupt {
+        PositiveEdge = gpio_int_type_t::GPIO_INTR_POSEDGE,
+        NegativeEdge = gpio_int_type_t::GPIO_INTR_NEGEDGE,
+        AnyEdge = gpio_int_type_t::GPIO_INTR_ANYEDGE
+    };
+
+    GpioPort(gpio_num_t port);
+    ~GpioPort();
+
+    uint64_t GetReserved(void) { return _reservedPorts; }
+
+  protected:
+    const gpio_num_t _port;
+
+  private:
+    static uint64_t _reservedPorts;
+};
+
+/**
+ * @brief Input-Port
+ * @details Provides abstract reading functionality
+ */
+class InputPort : public GpioPort {
+  public:
+    InputPort(gpio_num_t port, GpioPort::PullResistor resistor = PullResistor::Floating);
+    void setLogic(GpioPort::Logic logic) {
+      _logic = logic;
+    }
+    ~InputPort() = default;
+
+    bool read(void);
+
+    private:
+      GpioPort::Logic _logic = GpioPort::Logic::Normal;
+};
+
+/**
+ * @brief Output-Port
+ * @details Provides abstract setting functionality
+ */
+class OutputPort : public GpioPort {
+  public:
+    OutputPort(gpio_num_t port, GpioPort::Logic mode = Logic::Normal);
+    ~OutputPort(){};
+
+    void write(int active);
+
+  private:
+    const GpioPort::Logic _mode = Normal;
+};
+
+/**
+ * @brief Interrupt-Port
+ */
+class TriggeredInput : public InputPort {
+  public:
+    TriggeredInput(gpio_num_t port,
+                   GpioPort::PullResistor resistor = GpioPort::PullResistor::Floating,
+                   TriggeredInput::Interrupt edge = TriggeredInput::Interrupt::AnyEdge);
+    ~TriggeredInput(){};
+
+    void RegisterIsr(gpio_isr_t pIsrHandle);
+
+  private:
+    static bool _isrServiceInitiated;
+};

components/board_io/src/gpioPort.cpp

@@ -0,0 +1,98 @@
+/**
+ * @file gpioPort.cpp
+ * @author Gustice
+ * @brief C++ Wrapper for GPIO port
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "gpioPort.hpp"
+#include "esp_log.h"
+
+///
+/// Gpio - Implementation
+///
+uint64_t GpioPort::_reservedPorts = 0;
+
+GpioPort::GpioPort(gpio_num_t port) : _port(port) {
+    uint64_t p = 1ULL << port;
+    if ((_reservedPorts & p) != 0) {
+        ESP_LOGW("HAL", "GpioPort Port %d is already in use!", port);
+    }
+
+    _reservedPorts |= p;
+}
+
+GpioPort::~GpioPort() {
+    uint64_t p = 1ULL << _port;
+    _reservedPorts &= ~p;
+}
+
+///
+/// InputPort - Implementation
+///
+InputPort::InputPort(gpio_num_t port, GpioPort::PullResistor resistor) : GpioPort(port) {
+    gpio_config_t config = {
+        .pin_bit_mask = 1ULL << port,
+        .mode = GPIO_MODE_INPUT,
+        .pull_up_en = resistor == PullUp ? GPIO_PULLUP_ENABLE : GPIO_PULLUP_DISABLE,
+        .pull_down_en = resistor == PullDown ? GPIO_PULLDOWN_ENABLE : GPIO_PULLDOWN_DISABLE,
+        .intr_type = GPIO_INTR_DISABLE,
+    };
+
+    ESP_ERROR_CHECK(gpio_config(&config));
+}
+
+bool InputPort::read(void) {
+    auto value = gpio_get_level(_port);
+    return ((value ^ _logic) & 0x01) == 1;
+}
+
+///
+/// TriggeredInput - Implementation
+///
+bool TriggeredInput::_isrServiceInitiated = false;
+
+TriggeredInput::TriggeredInput(gpio_num_t port, GpioPort::PullResistor resistor,
+                               TriggeredInput::Interrupt edge)
+    : InputPort(port, resistor) {
+    assert(gpio_set_intr_type(_port, (gpio_int_type_t)edge) == ESP_OK);
+
+    if (!_isrServiceInitiated) {
+        _isrServiceInitiated = true;
+        esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL2);
+        if (err == ESP_OK) {
+            ESP_LOGI("HAL", "ISR-service installed successfully for gpio-port");
+        } else {
+            ESP_LOGE("HAL", "ISR-service failed to install. Code '%s'", esp_err_to_name(err));
+        }
+    }
+}
+
+void TriggeredInput::RegisterIsr(gpio_isr_t pIsrHandle) {
+    ESP_LOGI("HAL", "Hooking ISR-Function to gpio-port %d", _port);
+    gpio_isr_handler_add(_port, pIsrHandle, nullptr);
+}
+
+///
+/// OutputPort - Implementation
+///
+OutputPort::OutputPort(gpio_num_t port, GpioPort::Logic mode) : GpioPort(port), _mode(mode) {
+    gpio_config_t io_conf = {
+        .pin_bit_mask = 1ULL << port,
+        .mode = GPIO_MODE_OUTPUT,
+        .pull_up_en = GPIO_PULLUP_DISABLE,
+        .pull_down_en = GPIO_PULLDOWN_DISABLE,
+        .intr_type = GPIO_INTR_DISABLE,
+    };
+    gpio_config(&io_conf);
+}
+
+void OutputPort::write(int active) {
+    if (_mode == GpioPort::Normal) {
+        gpio_set_level(_port, active & 1);
+    } else {
+        active ^= 1;
+        gpio_set_level(_port, active & 1);
+    }
+}

components/lighting/CMakeLists.txt

@@ -2,7 +2,8 @@ idf_component_register(
     SRC_DIRS "src"
     INCLUDE_DIRS "include"
     REQUIRES freeRtosApi 
+        project_bases
         esp_common 
         esp_driver_gpio 
-        esp_driver_i2c 
+        esp_driver_i2c
 )

components/movement/CMakeLists.txt

@@ -0,0 +1,11 @@
+idf_component_register(
+    SRC_DIRS "src"
+    INCLUDE_DIRS "include"
+    REQUIRES freeRtosApi 
+        project_bases
+        board_io
+        esp_common 
+        esp_driver_gpio 
+        esp_driver_ledc
+        esp_driver_gptimer
+)

components/movement/include/StepperDrv8833.hpp

@@ -0,0 +1,42 @@
+/**
+ * @file StepperDrv8833.hpp
+ * @author Gustice
+ * @brief Software-driver for Stepper driver DRV8833
+ * @details https://www.ti.com/lit/ds/symlink/drv8833.pdf?ts=1726872460958&ref_url=https%253A%252F%252Fwww.ti.com%252Fproduct%252Fde-de%252FDRV8833
+ *
+ * @copyright Copyright (c) 2024
+ */
+#pragma once
+
+#include "stepperDrv.hpp"
+#include <array>
+#include <vector>
+
+class StepperDriverDrv8833 : public StepperDriver {
+  public:
+    struct Pins {
+        ledc_timer_t timer;
+        std::array<ledc_channel_t, 4> channels;
+        std::array<gpio_num_t, 4> pins;
+    };
+
+    StepperDriverDrv8833(Pins pins, int resolution);
+    StepCplx fullStep(int8_t cnt) override;
+    StepCplx step(int8_t cnt) override;
+    StepCplx setPos(uint8_t pos) override;
+    uint8_t getSubPos() {
+        return _position;
+    }
+
+  private:
+    PwmPort _pwmA1;
+    PwmPort _pwmA2;
+    PwmPort _pwmB1;
+    PwmPort _pwmB2;
+
+    std::vector<uint8_t> _outA1;
+    std::vector<uint8_t> _outA2;
+    std::vector<uint8_t> _outB1;
+    std::vector<uint8_t> _outB2;
+    void setOutput(uint8_t pos) override;
+};

components/movement/include/gpTimer.hpp

@@ -0,0 +1,82 @@
+/**
+ * @file gpTimer.hpp
+ * @author Gustice
+ * @brief ESP-hardware-timer class
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#pragma once
+
+#include "driver/gptimer.h"
+#include "esp_attr.h"
+#include "esp_log.h"
+#include "portBase.hpp"
+
+/**
+ * @brief Base class for tasks
+ */
+class HwTimerBase : public PortBase {
+  public:
+    enum Mode {
+        OneShot = 0,
+        Periodic
+    };
+
+    void setup(int thresh, Mode mode);
+    void start();
+    void stop();
+
+  protected:
+    gptimer_handle_t gpTimer{};
+
+    HwTimerBase(gptimer_alarm_cb_t callback);
+    ~HwTimerBase();
+};
+
+/**
+ * @brief Simple Timer without callback
+ */
+class HwTimer : public HwTimerBase {
+  public:
+    using CallSignature = bool (*)(HwTimer &);
+
+    HwTimer(CallSignature call)
+        : HwTimerBase(CallFrame), _call{call} {
+    }
+
+  private:
+    CallSignature _call;
+
+    static bool IRAM_ATTR CallFrame(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *_this) {
+        auto &tmr = *reinterpret_cast<HwTimer *>(_this);
+        return tmr._call(tmr);
+    }
+};
+
+/**
+ * @brief Timer with callback parameter
+ * @tparam T Type of task parameter on call
+ */
+template <typename T>
+class HwTimerT : public HwTimerBase {
+    using CallSignatureT = bool (*)(HwTimerT<T> &, T);
+
+  public:
+    HwTimerT(CallSignatureT call, const T &param)
+        : HwTimerBase(CallFrame), _param(call, param) {
+    }
+
+  private:
+    template <typename TS>
+    struct Param {
+        CallSignatureT call;
+        const TS &arg;
+    };
+    static bool IRAM_ATTR CallFrame(gptimer_handle_t timer, const gptimer_alarm_event_data_t *edata, void *_this) {
+        auto &tmr = *reinterpret_cast<HwTimerT *>(_this);
+        auto &param = tmr._param;
+        return param.call(tmr, param.arg);
+    }
+    Param<T> _param;
+};

components/movement/include/pwmPort.hpp

@@ -0,0 +1,34 @@
+/**
+ * @file pwmPort.hpp
+ * @author Gustice
+ * @brief C++ Wrapper for PWM-output
+ *
+ * @copyright Copyright (c) 2024
+ */
+#pragma once
+
+#include "driver/gpio.h"
+#include "driver/ledc.h"
+#include "portBase.hpp"
+#include <cstdint>
+
+/**
+ * @brief PWM-Class
+ * @details Provides Write to PWM-Port
+ */
+class PwmPort : public PortBase {
+  public:
+    PwmPort(ledc_timer_t timer, ledc_channel_t channel, gpio_num_t output, ledc_timer_bit_t resolution, uint32_t frequency);
+    ~PwmPort() = default;
+
+    void write(float duty);
+    void writeRaw(uint16_t duty);
+
+  protected:
+    const uint32_t FullScale;
+    const ledc_timer_t _timer;
+    const ledc_channel_t _channel;
+    const gpio_num_t _port;
+
+  private:
+};

components/movement/include/stepperDrv.hpp

@@ -0,0 +1,36 @@
+/**
+ * @file stepperDrv.hpp
+ * @author Gustice
+ * @brief Abstraction for Stepper driver
+ *
+ * @copyright Copyright (c) 2024
+ */
+#pragma once
+
+#include "gpioPort.hpp"
+#include "pwmPort.hpp"
+#include <array>
+#include <vector>
+
+class StepperDriver {
+  public:
+    struct StepCplx {
+        int8_t increment{};
+        uint8_t subPos{};
+    };
+
+    StepperDriver(int resolution)
+        : Resolution(resolution), Quad(Resolution / 4) {}
+    virtual StepCplx fullStep(int8_t cnt) = 0;
+    virtual StepCplx step(int8_t cnt) = 0;
+    virtual StepCplx setPos(uint8_t pos) = 0;
+    uint8_t getSubPos() {
+        return _position;
+    }
+
+  protected:
+    const uint8_t Resolution;
+    const uint8_t Quad;
+    int8_t _position{};
+    virtual void setOutput(uint8_t pos) = 0;
+};

components/movement/include/stepperMotor.hpp

@@ -0,0 +1,311 @@
+/**
+ * @file stepperMotor.hpp
+ * @author Gustice
+ * @brief Stepper Motor device class
+ *
+ * @copyright Copyright (c) 2024
+ */
+#pragma once
+
+#include "gpTimer.hpp"
+#include "gpioPort.hpp"
+#include "stepperDrv.hpp"
+#include <Task.hpp>
+#include <cmath>
+#include <memory>
+
+struct Profile {
+    std::vector<uint8_t> steps;
+};
+
+class StepperMotor {
+    enum Mode {
+        Idle = 0,
+        HomingFromSensor, // if home sensor already active
+        HomingToSensorBegin,
+        HomingToSensorEnd,
+        Homing,
+        Run,
+        CrawlToEnd,
+    };
+
+  public:
+    template <int MAX, int PREC>
+    struct Position {
+        uint16_t position{};
+        uint16_t subPos{};
+        Position &operator+(Position &lhs) {
+            auto v1 = getFixPoint();
+            auto v2 = lhs.getFixPoint();
+            auto v = v1 + v2;
+            if (v > (MAX << 16)) {
+                v -= (MAX << 16);
+            }
+            position = getPosition(v);
+            subPos = getSubPos(v);
+            return *this;
+        }
+
+        Position &operator-(Position &lhs) {
+            auto v1 = getFixPoint();
+            auto v2 = lhs.getFixPoint();
+            auto v = v1 - v2;
+            if (v < (MAX << 16)) {
+                v += (MAX << 16);
+            }
+            position = getPosition(v);
+            subPos = getSubPos(v);
+            return *this;
+        }
+
+        Position &operator+(StepperDriver::StepCplx &pos) {
+            position += pos.increment;
+            if (position > MAX) {
+                position -= MAX;
+            } else if (position < 0) {
+                position += MAX;
+            } else {
+                // nothing to do here
+            }
+            subPos += pos.subPos;
+            return *this;
+        }
+
+        Position &halv() {
+            auto v1 = getFixPoint();
+            auto v = v1 / 2;
+            position = getPosition(v);
+            subPos = getSubPos(v);
+            return *this;
+        }
+
+        float getScaled() {
+            auto scaled = (float)position / MAX;
+            // scaled += (float)subPos / (1 << PREC);
+            return scaled;
+        }
+
+      private:
+        int32_t getFixPoint() {
+            int32_t value = (((int32_t)position) << 16) | (subPos << (16 - PREC));
+            return value;
+        }
+        uint16_t getPosition(int32_t value) {
+            auto pos = (value >> 16) & 0xFFFF;
+            return pos;
+        }
+        uint16_t getSubPos(int32_t value) {
+            auto pos = ((value & 0xFFFF) >> (16 - PREC));
+            return pos;
+        }
+    };
+
+    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)
+        : _config(config), _driver(driver), _initSensor(initSensor),
+          _timer(TimerCallParam, *this) {
+        _timer.setup(1'000, HwTimerBase::Mode::Periodic);
+        _timer.start();
+    }
+    ~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);
+        // }
+
+        int fullStep = _config.resolution / 4;
+        _driver.setPos(0);
+
+        if (_initSensor.read()) {
+            ESP_LOGI("Stepper", "Need to move away from home");
+            // MoveAway
+            while (_initSensor.read()) {
+                (void)_driver.step(-fullStep);
+                fos::Task::delayTicks(1);
+            }
+            for (size_t i = 0; i < 8; i++) // Drive another 4 Steps away
+            {
+                (void)_driver.step(-fullStep);
+                fos::Task::delayTicks(1);
+            }
+        }
+
+        ESP_LOGI("Stepper", "Fast find right side of home sensor");
+        // Find right Endpoint
+        while (not _initSensor.read()) {
+            (void)_driver.step(fullStep);
+            fos::Task::delayTicks(1);
+        }
+        while (_initSensor.read()) { // move away
+            (void)_driver.step(-fullStep);
+            fos::Task::delayTicks(1);
+        }
+        ESP_LOGI("Stepper", "Find right side of home sensor");
+        while (not _initSensor.read()) {
+            (void)_driver.step(1);
+            fos::Task::delayTicks(1);
+        }
+        int pos{};
+        resetPosition();
+        ESP_LOGI("Stepper", "Find end of sensor");
+        while (_initSensor.read()) {
+            (void)_driver.step(1);
+            pos++;
+            fos::Task::delayTicks(1);
+        }
+        for (size_t i = 0; i < 32; i++) // Drive another 4 Steps away
+        {
+            (void)_driver.step(1);
+            pos++;
+            fos::Task::delayTicks(1);
+        }
+
+        ESP_LOGI("Stepper", "Find left side of home sensor");
+        while (not _initSensor.read()) {
+            (void)_driver.step(-1);
+            pos--;
+            fos::Task::delayTicks(1);
+        }
+
+        ESP_LOGI("Stepper", "Move back to mid position %d", pos / 2);
+        for (size_t i = 0; i < pos / 2; i++) {
+            (void)_driver.step(-1);
+            fos::Task::delayTicks(1);
+        }
+        ESP_LOGI("Stepper", "Reset position");
+        resetPosition();
+    }
+
+    void demoFullTurn() {
+        auto steps = _config.resolution * _config.steps;
+        ESP_LOGI("Stepper", "Running %i * %i steps for full turn", _config.resolution, _config.steps);
+
+        ESP_LOGI("Stepper", "Start is %f.2", getScaledPos());
+        for (size_t i = 0; i < _config.resolutionQuad * _config.steps; i++) {
+            auto delta = _driver.step(1);
+            _indexPos = _indexPos + delta;
+            fos::Task::delayTicks(1);
+        }
+        ESP_LOGI("Stepper", "End is %f.2", getScaledPos());
+    }
+
+    void createDemoProfile();
+    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;
+    }
+
+    bool isIdle() {
+        return _mode == Mode::Idle;
+    }
+
+    float getScaledPos() {
+        return _indexPos.getScaled();
+    }
+
+    void resetPosition() {
+        _indexPos = Position<50, 5>();
+    }
+
+  private:
+    Config &_config;
+    StepperDriver &_driver;
+    InputPort &_initSensor;
+    HwTimerT<StepperMotor &> _timer;
+    Mode _mode{};
+    int _step{};
+    Trajectory _trajectory{};
+    Position<50, 5> _indexPos{};
+
+    static bool IRAM_ATTR TimerCallParam(HwTimerT<StepperMotor &> &, StepperMotor &stepper) {
+        switch (stepper._mode) {
+        case Mode::HomingFromSensor: {
+            if (stepper._initSensor.read() == false) {
+                stepper._mode = HomingToSensorBegin;
+                break;
+            }
+            (void)stepper._driver.step(1);
+        } break;
+
+        case Mode::HomingToSensorBegin: {
+            if (stepper._initSensor.read()) {
+                stepper._mode = HomingToSensorEnd;
+                stepper._indexPos = {0, stepper._driver.getSubPos()};
+                break;
+            }
+            (void)stepper._driver.step(1);
+        } break;
+
+        case Mode::HomingToSensorEnd: {
+            if (stepper._initSensor.read() == false) {
+                stepper._mode = Homing;
+                stepper._indexPos = stepper._indexPos.halv();
+                break;
+            }
+            auto delta = stepper._driver.step(1);
+            stepper._indexPos = stepper._indexPos + delta;
+        } break;
+
+        case Mode::Homing: {
+            if (stepper._indexPos.position == 0) {
+                stepper._mode = Idle;
+                break;
+            }
+            auto delta = stepper._driver.step(-1);
+            stepper._indexPos = stepper._indexPos + delta;
+        } break;
+
+        case Mode::Run: {
+            auto stp = stepper._trajectory->steps[stepper._step];
+            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::Idle:
+        default:
+            break;
+        }
+
+        return true;
+    }
+};

components/movement/src/gpTimer.cpp

@@ -0,0 +1,64 @@
+/**
+ * @file gpTimer.cpp
+ * @author Gustice
+ * @brief HW-Timer class implementation
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "gpTimer.hpp"
+#include "esp_log.h"
+
+HwTimerBase::HwTimerBase(gptimer_alarm_cb_t callback) {
+    ESP_LOGI("TMR", "Create timer handle");
+    gptimer_config_t timer_config = {
+        .clk_src = GPTIMER_CLK_SRC_DEFAULT,
+        .direction = GPTIMER_COUNT_UP,
+        .resolution_hz = 1000000, // 1MHz, 1 tick=1us
+    };
+    ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gpTimer));
+
+    gptimer_event_callbacks_t cbs = {
+        .on_alarm = callback,
+    };
+    ESP_ERROR_CHECK(gptimer_register_event_callbacks(gpTimer, &cbs, this));
+}
+
+void HwTimerBase::setup(int thresh, Mode mode) {
+    ESP_LOGI("TMR", "Setup timer");
+    ESP_ERROR_CHECK(gptimer_enable(gpTimer));
+
+    gptimer_alarm_config_t cfg{};
+
+    switch (mode) {
+    case Mode::OneShot:
+        cfg.alarm_count = thresh;
+        break;
+    case Mode::Periodic:
+        cfg.reload_count = 0,
+        cfg.alarm_count = thresh;
+        cfg.flags.auto_reload_on_alarm = true;
+        break;
+    default:
+        ESP_LOGE("TMR", "Invalid mode %d", mode);
+        break;
+    }
+    ESP_ERROR_CHECK(gptimer_set_alarm_action(gpTimer, &cfg));
+}
+
+void HwTimerBase::start() {
+    ESP_ERROR_CHECK(gptimer_start(gpTimer));
+}
+
+void HwTimerBase::stop() {
+    ESP_LOGI("TMR", "Stop timer");
+    ESP_ERROR_CHECK(gptimer_stop(gpTimer));
+}
+
+HwTimerBase::~HwTimerBase() {
+    stop();
+    ESP_LOGI("TMR", "Disable timer");
+    ESP_ERROR_CHECK(gptimer_disable(gpTimer));
+    ESP_LOGI("TMR", "Delete timer");
+    ESP_ERROR_CHECK(gptimer_del_timer(gpTimer));
+}

components/movement/src/pwmPort.cpp

@@ -0,0 +1,48 @@
+/**
+ * @file pwmPort.cpp
+ * @author Gustice
+ * @brief C++ Wrapper for PWM-output
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "pwmPort.hpp"
+#include "esp_log.h"
+#include "esp_system.h"
+#include <cassert>
+#include <cmath>
+
+PwmPort::PwmPort(ledc_timer_t timer, ledc_channel_t channel, gpio_num_t output, ledc_timer_bit_t resolution, uint32_t frequency)
+    : FullScale((1 << resolution)), _timer(timer), _channel(channel), _port(output) {
+    ledc_timer_config_t ledc_timer = {
+        .speed_mode = LEDC_LOW_SPEED_MODE,
+        .duty_resolution = resolution,
+        .timer_num = timer,
+        .freq_hz = frequency,
+        .clk_cfg = LEDC_AUTO_CLK,
+    };
+    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
+
+    // Prepare and then apply the LEDC PWM channel configuration
+    ledc_channel_config_t ledc_channel = {
+        .gpio_num = output,
+        .speed_mode = LEDC_LOW_SPEED_MODE,
+        .channel = channel,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = timer,
+        .duty = 0,
+        .hpoint = 0,
+    };
+    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
+}
+
+void PwmPort::write(float duty) {
+    // duty <= 1.0F;
+    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, _channel, duty * FullScale));
+    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, _channel));
+}
+
+void PwmPort::writeRaw(uint16_t duty) {
+    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, _channel, duty));
+    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, _channel));
+}

components/movement/src/stepperDrv8833.cpp

@@ -0,0 +1,139 @@
+/**
+ * @file stepperDrv8833.cpp
+ * @author Gustice
+ * @brief Software-driver for Stepper driver DRV8833
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "stepperDrv8833.hpp"
+
+#include "esp_log.h"
+#include <climits>
+#include <cmath>
+
+void populateLookUps(int resolution,
+                     std::vector<uint8_t> &a1, std::vector<uint8_t> &a2,
+                     std::vector<uint8_t> &b1, std::vector<uint8_t> &b2) {
+    float step = 2.0F * std::numbers::pi / resolution;
+    const uint16_t FullScale = 1 << LEDC_TIMER_7_BIT;
+    
+    for (size_t i = 0; i < resolution; i++) {
+        
+        auto cos = std::cos(step * i);
+        if (cos > 0)
+        {
+            // 1 :   0 = Forward
+            // 1 : PWM = Forward PWM, slow decay
+            a1.push_back(FullScale);
+            a2.push_back(FullScale - (cos * FullScale));
+        } else {
+            //   0 : 1 = Reverse
+            // PWM : 1 = Reverse PWM, slow decay
+            a1.push_back(FullScale - (-cos * FullScale));
+            a2.push_back(FullScale);
+        }
+
+        auto sin = std::sin(step * i);
+        if (sin > 0)
+        {
+            // 1 :   0 = Forward
+            // 1 : PWM = Forward PWM, slow decay
+            b1.push_back(FullScale);
+            b2.push_back(FullScale - (sin * FullScale));
+        } else {
+            //   0 : 1 = Reverse
+            // PWM : 1 = Reverse PWM, slow decay
+            b1.push_back(FullScale - ((-sin * FullScale)));
+            b2.push_back(FullScale);
+        }
+    };
+}
+
+StepperDriverDrv8833::StepperDriverDrv8833(Pins pins, int resolution)
+    : 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)
+      {
+    if ((Resolution % 4) != 0) {
+        ESP_LOGE("Stepper", "Resolution must be multiple of 4");
+    }
+
+    populateLookUps(resolution, _outA1, _outA2, _outB1, _outB2);
+
+    char buffer[resolution * 2 + 10];
+    ESP_LOGD("Stepper", "Output Data:");
+    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA1.data(), _outA1.size(), ESP_LOG_DEBUG);
+    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outA2.data(), _outA2.size(), ESP_LOG_DEBUG);
+    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB1.data(), _outB1.size(), ESP_LOG_DEBUG);
+    ESP_LOG_BUFFER_HEXDUMP("Stepper", _outB2.data(), _outB2.size(), ESP_LOG_DEBUG);
+
+    step(0);
+}
+
+StepperDriverDrv8833::StepCplx StepperDriverDrv8833::fullStep(int8_t cnt) {
+    if (cnt > 0) {
+        cnt = 1;
+    } else {
+        cnt = -1;
+    }
+
+    int pos = (_position / Quad + cnt) % 4;
+    pos = pos * Quad;
+    return setPos(pos);
+}
+
+StepperDriverDrv8833::StepCplx StepperDriverDrv8833::step(int8_t cnt) {
+    if (cnt == 0) {
+        return {0, (uint8_t)_position};
+    }
+    if (cnt > Resolution) {
+        // ESP_LOGE("Stepper", "microstep exceeds four steps");
+    }
+    if (cnt > Quad) {
+        // ESP_LOGW("Stepper", "microstep exceeds quadrature step");
+    }
+
+    int8_t increment{};
+    auto pos = _position + cnt;
+    if (pos < 0) {
+        increment--;
+        pos += Resolution;
+    } else if (pos >= Resolution) {
+        increment++;
+        pos -= Resolution;
+    } else {
+        // in that case ... nothing
+    }
+    setOutput(pos);
+    return {increment, (uint8_t)_position};
+}
+
+StepperDriverDrv8833::StepCplx StepperDriverDrv8833::setPos(uint8_t pos) {
+    pos = (pos) % Resolution;
+
+    int8_t increment{};
+    if (pos - _position < 0) {
+        increment--;
+    } else if (pos - _position > Resolution) {
+        increment++;
+    } else {
+        // in that case ... nothing
+    }
+
+    setOutput(pos);
+    return {increment, (uint8_t)_position};
+}
+
+void StepperDriverDrv8833::setOutput(uint8_t pos) {
+    if (pos >= Resolution) {
+        // ESP_LOGE("Stepper", "d exceeds micro-stepping size");
+    }
+    _position = pos;
+    _pwmA1.writeRaw(_outA1[pos]);
+    _pwmA2.writeRaw(_outA2[pos]);
+    _pwmB1.writeRaw(_outB1[pos]);
+    _pwmB2.writeRaw(_outB2[pos]);
+}

components/movement/src/stepperMotor.cpp

@@ -0,0 +1,288 @@
+/**
+ * @brief stepperMotor.cpp
+ * @author Gustice
+ * @brief Stepper Motor device class
+ *
+ * @copyright Copyright (c) 2024
+ */
+
+#include "stepperMotor.hpp"
+#include "esp_log.h"
+#include <cmath>
+#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 pnt = getStopDistance(_aMax, _wMax);
+        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 - _pVec.back()) / _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));
+}
+
+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);
+
+    ProfileGenerator generator(_config, 0.001F, accel, speed);
+
+    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;
+    }
+    auto profile = call(angle);
+    // 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));
+}

components/project_bases/CMakeLists.txt

@@ -0,0 +1,4 @@
+idf_component_register(
+    SRC_DIRS "."
+    INCLUDE_DIRS "."
+)

components/project_bases/portBase.hpp

@@ -7,11 +7,9 @@
  */
 #pragma once
 
-class PortBase {
+class PortBase { // Forbid Copy and Move assignments
   public:
     PortBase(/* args */) {};
-
-    // Forbid Copy and Move assignments
     PortBase(const PortBase &other) = delete;
     PortBase &operator=(const PortBase &other) = delete;
 };

main/watch_main.cpp

@@ -29,6 +29,7 @@
 // Component header
 #include "Task.hpp"
 #include "PwmExpanderPca9685.hpp"
+#include "StepperDrv8833.hpp"
 
 static const char *TAG = "Watch";
 static esp_eth_handle_t s_eth_handle = NULL;
@@ -114,8 +115,8 @@ void LightingTask() {
     ESP_LOGI(TAG, "Start Lighting");
 
     static I2cMaster master(I2C_NUM_0, 
-        /*sda=*/ GPIO_NUM_13, 
+        /*sda=*/ GPIO_NUM_0, 
-        /*scl=*/ GPIO_NUM_16);
+        /*scl=*/ GPIO_NUM_4);
     // static PmwExpanderPca9685 pca9685(master);
     static PmwExpanderPca9685 ledDriver1(master, /*iverted=*/ true);
     static PmwExpanderPca9685 ledDriver2(master, /*iverted=*/ true, 0x41); 
@@ -139,6 +140,25 @@ void LightingTask() {
     }
 }
 
+void MovementTask() {
+    ESP_LOGI(TAG, "Start Stepper");
+    constexpr int MicroSteps = 32;
+    StepperDriverDrv8833::Pins stepperPins{
+        LEDC_TIMER_0,
+        {LEDC_CHANNEL_0, LEDC_CHANNEL_1, LEDC_CHANNEL_2, LEDC_CHANNEL_3},
+        {GPIO_NUM_16, GPIO_NUM_15, GPIO_NUM_14, GPIO_NUM_13},
+    };
+    StepperDriverDrv8833 stepperDriver(stepperPins, MicroSteps);
+    stepperDriver.setPos(0);
+
+    while (true) {
+        stepperDriver.step(1);
+        stepperDriver.getSubPos();
+        fos::Task::delayMs(100);
+    }
+}
+
+
 void app_main(void)
 {
     esp_err_t ret = nvs_flash_init();
@@ -150,4 +170,5 @@ void app_main(void)
     // ESP_ERROR_CHECK(esp_event_loop_create_default());
     initialize_ethernet();
     static fos::Task lightingTask(LightingTask, "Lights", 0x4000uL);
+    static fos::Task movementTask(MovementTask, "Motor", 0x4000uL);
 }