From 0181c72a9df5c74f9079442919d519d59f8e8481 Mon Sep 17 00:00:00 2001 From: Jakob Gegeniger Date: Sat, 16 Nov 2024 20:04:38 +0100 Subject: [PATCH] PWM based motor driver added from other project --- components/board_io/CMakeLists.txt | 10 + components/board_io/include/gpioPort.hpp | 95 ++++++ components/board_io/src/gpioPort.cpp | 98 ++++++ components/lighting/CMakeLists.txt | 3 +- components/movement/CMakeLists.txt | 11 + .../movement/include/StepperDrv8833.hpp | 42 +++ components/movement/include/gpTimer.hpp | 82 +++++ components/movement/include/pwmPort.hpp | 34 ++ components/movement/include/stepperDrv.hpp | 36 ++ components/movement/include/stepperMotor.hpp | 311 ++++++++++++++++++ components/movement/src/gpTimer.cpp | 64 ++++ components/movement/src/pwmPort.cpp | 48 +++ components/movement/src/stepperDrv8833.cpp | 139 ++++++++ components/movement/src/stepperMotor.cpp | 288 ++++++++++++++++ components/project_bases/CMakeLists.txt | 4 + .../include => project_bases}/portBase.hpp | 4 +- main/watch_main.cpp | 25 +- 17 files changed, 1288 insertions(+), 6 deletions(-) create mode 100644 components/board_io/CMakeLists.txt create mode 100644 components/board_io/include/gpioPort.hpp create mode 100644 components/board_io/src/gpioPort.cpp create mode 100644 components/movement/CMakeLists.txt create mode 100644 components/movement/include/StepperDrv8833.hpp create mode 100644 components/movement/include/gpTimer.hpp create mode 100644 components/movement/include/pwmPort.hpp create mode 100644 components/movement/include/stepperDrv.hpp create mode 100644 components/movement/include/stepperMotor.hpp create mode 100644 components/movement/src/gpTimer.cpp create mode 100644 components/movement/src/pwmPort.cpp create mode 100644 components/movement/src/stepperDrv8833.cpp create mode 100644 components/movement/src/stepperMotor.cpp create mode 100644 components/project_bases/CMakeLists.txt rename components/{lighting/include => project_bases}/portBase.hpp (83%) diff --git a/components/board_io/CMakeLists.txt b/components/board_io/CMakeLists.txt new file mode 100644 index 0000000..45e5cff --- /dev/null +++ b/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 +) diff --git a/components/board_io/include/gpioPort.hpp b/components/board_io/include/gpioPort.hpp new file mode 100644 index 0000000..586e27c --- /dev/null +++ b/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; +}; diff --git a/components/board_io/src/gpioPort.cpp b/components/board_io/src/gpioPort.cpp new file mode 100644 index 0000000..43de73f --- /dev/null +++ b/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); + } +} diff --git a/components/lighting/CMakeLists.txt b/components/lighting/CMakeLists.txt index ca9424d..352647a 100644 --- a/components/lighting/CMakeLists.txt +++ b/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 ) diff --git a/components/movement/CMakeLists.txt b/components/movement/CMakeLists.txt new file mode 100644 index 0000000..c431977 --- /dev/null +++ b/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 +) diff --git a/components/movement/include/StepperDrv8833.hpp b/components/movement/include/StepperDrv8833.hpp new file mode 100644 index 0000000..1cb67a9 --- /dev/null +++ b/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 +#include + +class StepperDriverDrv8833 : public StepperDriver { + public: + struct Pins { + ledc_timer_t timer; + std::array channels; + std::array 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 _outA1; + std::vector _outA2; + std::vector _outB1; + std::vector _outB2; + void setOutput(uint8_t pos) override; +}; \ No newline at end of file diff --git a/components/movement/include/gpTimer.hpp b/components/movement/include/gpTimer.hpp new file mode 100644 index 0000000..cfd1552 --- /dev/null +++ b/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(_this); + return tmr._call(tmr); + } +}; + +/** + * @brief Timer with callback parameter + * @tparam T Type of task parameter on call + */ +template +class HwTimerT : public HwTimerBase { + using CallSignatureT = bool (*)(HwTimerT &, T); + + public: + HwTimerT(CallSignatureT call, const T ¶m) + : HwTimerBase(CallFrame), _param(call, param) { + } + + private: + template + 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(_this); + auto ¶m = tmr._param; + return param.call(tmr, param.arg); + } + Param _param; +}; diff --git a/components/movement/include/pwmPort.hpp b/components/movement/include/pwmPort.hpp new file mode 100644 index 0000000..7ea263c --- /dev/null +++ b/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 + +/** + * @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: +}; diff --git a/components/movement/include/stepperDrv.hpp b/components/movement/include/stepperDrv.hpp new file mode 100644 index 0000000..1eb96c9 --- /dev/null +++ b/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 +#include + +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; +}; \ No newline at end of file diff --git a/components/movement/include/stepperMotor.hpp b/components/movement/include/stepperMotor.hpp new file mode 100644 index 0000000..a438e07 --- /dev/null +++ b/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 +#include +#include + +struct Profile { + std::vector steps; +}; + +class StepperMotor { + enum Mode { + Idle = 0, + HomingFromSensor, // if home sensor already active + HomingToSensorBegin, + HomingToSensorEnd, + Homing, + Run, + CrawlToEnd, + }; + + public: + template + 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; + + 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 _timer; + Mode _mode{}; + int _step{}; + Trajectory _trajectory{}; + Position<50, 5> _indexPos{}; + + static bool IRAM_ATTR TimerCallParam(HwTimerT &, 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; + } +}; diff --git a/components/movement/src/gpTimer.cpp b/components/movement/src/gpTimer.cpp new file mode 100644 index 0000000..d129e38 --- /dev/null +++ b/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)); +} diff --git a/components/movement/src/pwmPort.cpp b/components/movement/src/pwmPort.cpp new file mode 100644 index 0000000..8a1c651 --- /dev/null +++ b/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 +#include + +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)); +} diff --git a/components/movement/src/stepperDrv8833.cpp b/components/movement/src/stepperDrv8833.cpp new file mode 100644 index 0000000..708df75 --- /dev/null +++ b/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 +#include + +void populateLookUps(int resolution, + std::vector &a1, std::vector &a2, + std::vector &b1, std::vector &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]); +} diff --git a/components/movement/src/stepperMotor.cpp b/components/movement/src/stepperMotor.cpp new file mode 100644 index 0000000..31b7da8 --- /dev/null +++ b/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 +#include +#include + +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 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 _pVec; + // std::vector _wVec; + // std::vector _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 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 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 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(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(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 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(std::move(profile)); + runProfile(std::move(pp)); +} \ No newline at end of file diff --git a/components/project_bases/CMakeLists.txt b/components/project_bases/CMakeLists.txt new file mode 100644 index 0000000..397aead --- /dev/null +++ b/components/project_bases/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + SRC_DIRS "." + INCLUDE_DIRS "." +) diff --git a/components/lighting/include/portBase.hpp b/components/project_bases/portBase.hpp similarity index 83% rename from components/lighting/include/portBase.hpp rename to components/project_bases/portBase.hpp index 17ff2bf..b5bf247 100644 --- a/components/lighting/include/portBase.hpp +++ b/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; }; diff --git a/main/watch_main.cpp b/main/watch_main.cpp index 4283812..fc7b259 100644 --- a/main/watch_main.cpp +++ b/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, - /*scl=*/ GPIO_NUM_16); + /*sda=*/ GPIO_NUM_0, + /*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); }