From cb4922f942f27b2704481b88069f41e3b7921336 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 8 Jun 2026 12:29:00 -0500 Subject: [PATCH 1/4] feat(motorgo-plink): Add motorgo plink BSP compoenent --- components/lsm6dso/README.md | 7 +- components/lsm6dso/include/lsm6dso.hpp | 11 +- components/lsm6dso/src/lsm6dso.cpp | 8 +- components/motorgo-plink/CMakeLists.txt | 6 + components/motorgo-plink/README.md | 37 ++ .../motorgo-plink/example/CMakeLists.txt | 21 + components/motorgo-plink/example/README.md | 52 +++ .../motorgo-plink/example/main/CMakeLists.txt | 2 + .../example/main/Kconfig.projbuild | 14 + .../example/main/motorgo_plink_example.cpp | 160 +++++++ .../motorgo-plink/example/sdkconfig.defaults | 1 + components/motorgo-plink/idf_component.yml | 33 ++ .../motorgo-plink/include/motorgo-plink.hpp | 393 ++++++++++++++++++ .../motorgo-plink/src/motorgo-plink.cpp | 381 +++++++++++++++++ doc/en/imu/lsm6dso.rst | 9 +- doc/en/motorgo_plink.rst | 37 ++ doc/en/motorgo_plink_example.md | 2 + 17 files changed, 1168 insertions(+), 6 deletions(-) create mode 100644 components/motorgo-plink/CMakeLists.txt create mode 100644 components/motorgo-plink/README.md create mode 100644 components/motorgo-plink/example/CMakeLists.txt create mode 100644 components/motorgo-plink/example/README.md create mode 100644 components/motorgo-plink/example/main/CMakeLists.txt create mode 100644 components/motorgo-plink/example/main/Kconfig.projbuild create mode 100644 components/motorgo-plink/example/main/motorgo_plink_example.cpp create mode 100644 components/motorgo-plink/example/sdkconfig.defaults create mode 100644 components/motorgo-plink/idf_component.yml create mode 100644 components/motorgo-plink/include/motorgo-plink.hpp create mode 100644 components/motorgo-plink/src/motorgo-plink.cpp create mode 100644 doc/en/motorgo_plink.rst create mode 100644 doc/en/motorgo_plink_example.md diff --git a/components/lsm6dso/README.md b/components/lsm6dso/README.md index 058c6aff8..3341fe0c3 100644 --- a/components/lsm6dso/README.md +++ b/components/lsm6dso/README.md @@ -3,7 +3,8 @@ [![Badge](https://components.espressif.com/components/espp/lsm6dso/badge.svg)](https://components.espressif.com/components/espp/lsm6dso) This is an espp component for the LSM6DSO 6-axis IMU (3-axis accelerometer + -3-axis gyroscope) from STMicroelectronics. It supports both I2C and SPI +3-axis gyroscope) from STMicroelectronics and compatible LSM6DS-family parts +used with the same basic accel/gyro register map. It supports both I2C and SPI interfaces, FIFO, interrupts, tap/event detection, and on-chip filtering. The driver is designed for use with the ESP-IDF and espp framework, and is modeled after the ICM42607 and MT6701 components. @@ -19,6 +20,10 @@ after the ICM42607 and MT6701 components. The [example](./example) shows how to use the `espp::Lsm6dso` component to initialize and communicate with an LSM6DSO 6-axis IMU. +For basic accel/gyro use, the driver also accepts the LSM6DS33 `WHO_AM_I` +value (`0x69`), which allows BSPs such as MotorGo Plink to reuse the same +component over I2C without a separate driver. + ## Documentation See the [documentation](https://esp-cpp.github.io/espp/imu/lsm6dso.html) for full API details. diff --git a/components/lsm6dso/include/lsm6dso.hpp b/components/lsm6dso/include/lsm6dso.hpp index cf79b4067..42d0c6692 100644 --- a/components/lsm6dso/include/lsm6dso.hpp +++ b/components/lsm6dso/include/lsm6dso.hpp @@ -4,7 +4,8 @@ #include "lsm6dso_detail.hpp" namespace espp { -/// @brief Class for the LSM6DSO 6-axis IMU (Accel + Gyro) +/// @brief Class for the LSM6DSO / compatible LSM6DS-family 6-axis IMUs (Accel +/// + Gyro) /// @tparam Interface The interface type (I2C or SPI) /// /// The LSM6DSO is a 6-axis IMU with a 3-axis accelerometer and 3-axis gyroscope. @@ -24,7 +25,10 @@ namespace espp { /// - Use get_orientation(), get_gravity_vector() for orientation and gravity vector if filter is /// set /// -/// \note This class is intended for use with the LSM6DSO and compatible variants. +/// \note This class is intended for use with the LSM6DSO and compatible +/// variants. The currently supported WHO_AM_I values include the +/// LSM6DSO/LSM6DSOX family IDs and the LSM6DS33 ID used on the MotorGo +/// Plink. template class Lsm6dso : public BasePeripheral { using BasePeripheral::set_address; @@ -43,6 +47,9 @@ class Lsm6dso : public BasePeripheral::Lsm6dso(const Config &config) template bool Lsm6dso::init(std::error_code &ec) { std::lock_guard lock(base_mutex_); auto device_id = get_device_id(ec); - if (device_id != 0x6C && device_id != 0x6A) { // LSM6DSO IDs + if (ec) { + logger_.error("Failed to read device ID: {}", ec.message()); + return false; + } + if (device_id != LSM6DSO_DEVICE_ID && device_id != LSM6DSOX_DEVICE_ID && + device_id != LSM6DS33_DEVICE_ID) { logger_.error("Invalid device ID: 0x{:02X}", device_id); + ec = std::make_error_code(std::errc::no_such_device); return false; } // configure the accel / gyro diff --git a/components/motorgo-plink/CMakeLists.txt b/components/motorgo-plink/CMakeLists.txt new file mode 100644 index 000000000..cfb3e2a8b --- /dev/null +++ b/components/motorgo-plink/CMakeLists.txt @@ -0,0 +1,6 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component esp_driver_gpio i2c led lsm6dso math mt6701 spi task + REQUIRED_IDF_TARGETS "esp32s3" + ) diff --git a/components/motorgo-plink/README.md b/components/motorgo-plink/README.md new file mode 100644 index 000000000..18b478a15 --- /dev/null +++ b/components/motorgo-plink/README.md @@ -0,0 +1,37 @@ +# MotorGo Plink Board Support Package (BSP) Component + +[![Badge](https://components.espressif.com/components/espp/motorgo-plink/badge.svg)](https://components.espressif.com/components/espp/motorgo-plink) + +The MotorGo Plink is a four-channel ESP32-S3 motor-control board from +Every Flavor Robotics. It combines: + +- four dual-PWM DC motor outputs +- four MT6701-compatible SSI encoder chip-selects on a shared bus +- four RC-servo signal pins +- one onboard LSM6DS33 IMU on the internal I2C bus +- a Qwiic I2C bus plus a second internal I2C bus +- user and status LEDs + +The `espp::MotorGoPlink` component provides a singleton board abstraction for +those documented pin mappings, plus convenient helpers for: + +- initializing and driving the four DC-motor channels with normalized speed + commands, where positive commands drive `pwm_a`, negative commands drive + `pwm_b`, and zero disables both outputs +- initializing the shared encoder SPI/SSI bus and creating four + `espp::Mt6701` encoder instances +- initializing the onboard LSM6DS33 IMU on the hidden I2C bus via the shared + `espp::Lsm6dso` driver +- accessing the four servo signal pins so you can attach your preferred + servo-control driver +- controlling the user/status LEDs with simple brightness setters or a Gaussian + breathing effect +- accessing the external Qwiic and internal I2C buses + +## Example + +The [example](./example) initializes the board, logs the MotorGo Plink pin map, +starts the onboard LEDs breathing, initializes the onboard IMU, and safely +keeps the motors stopped by default. Optional `menuconfig` flags let you enable +encoder polling and a slow motor sweep when you are connected to real hardware +and want to exercise the board interfaces. diff --git a/components/motorgo-plink/example/CMakeLists.txt b/components/motorgo-plink/example/CMakeLists.txt new file mode 100644 index 000000000..65646b1b4 --- /dev/null +++ b/components/motorgo-plink/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" + ) + +set( + COMPONENTS + "main logger motorgo-plink" + CACHE STRING + "List of components to include" + ) + +project(motorgo_plink_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/motorgo-plink/example/README.md b/components/motorgo-plink/example/README.md new file mode 100644 index 000000000..ac1bd4a0e --- /dev/null +++ b/components/motorgo-plink/example/README.md @@ -0,0 +1,52 @@ +# MotorGo Plink Example + +This example demonstrates how to use the `espp::MotorGoPlink` component to +initialize the hardware on the MotorGo Plink board. + +By default the example is intentionally safe: + +- it logs the documented motor, encoder, servo, I2C, and LED pin mappings +- it starts the user and status LEDs breathing +- it initializes the motor PWM outputs and keeps all four motors stopped + +Optional `menuconfig` flags let you also: + +- initialize and poll the four encoder inputs +- run a slow sinusoidal motor sweep across the four channels + +## How to use example + +### Hardware Required + +This example is designed for the MotorGo Plink board. + +The default configuration does not require motors or encoders to be connected. +If you enable the motor-sweep or encoder-polling options, connect the matching +hardware first. + +### Configuration + +Open the example configuration menu if you want to enable the optional hardware +exercise modes: + +```bash +idf.py menuconfig +``` + +Then open **MotorGo Plink Example Configuration** and enable any of: + +- **Enable encoder polling** +- **Enable motor sweep demo** + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +```bash +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type `Ctrl-]`.) diff --git a/components/motorgo-plink/example/main/CMakeLists.txt b/components/motorgo-plink/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/motorgo-plink/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/motorgo-plink/example/main/Kconfig.projbuild b/components/motorgo-plink/example/main/Kconfig.projbuild new file mode 100644 index 000000000..944b5cc3b --- /dev/null +++ b/components/motorgo-plink/example/main/Kconfig.projbuild @@ -0,0 +1,14 @@ +menu "MotorGo Plink Example Configuration" + config MOTORGO_PLINK_EXAMPLE_ENABLE_ENCODER_POLLING + bool "Enable encoder polling" + default n + help + Initialize the four encoder inputs and log their measured angles. + + config MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP + bool "Enable motor sweep demo" + default n + help + Drive all four motors with a slow sinusoidal speed command. Keep + this disabled unless the board is connected to a safe test setup. +endmenu diff --git a/components/motorgo-plink/example/main/motorgo_plink_example.cpp b/components/motorgo-plink/example/main/motorgo_plink_example.cpp new file mode 100644 index 000000000..66a003c21 --- /dev/null +++ b/components/motorgo-plink/example/main/motorgo_plink_example.cpp @@ -0,0 +1,160 @@ +#include +#include +#include +#include + +#include +#include +#include + +#include "logger.hpp" +#include "motorgo-plink.hpp" + +namespace { +static auto logger = espp::Logger({ + .tag = "MotorGoPlink Example", + .level = espp::Logger::Verbosity::INFO, +}); + +void log_board_info(espp::MotorGoPlink &board) { + auto qwiic = board.qwiic_pins(); + auto hidden_i2c = board.hidden_i2c_pins(); + auto leds = board.led_pins(); + auto enc_bus = board.encoder_bus_pins(); + auto encoder_cs = board.encoder_cs_pins(); + auto servos = board.servo_pins(); + + logger.info("Qwiic I2C: SDA={}, SCL={}", qwiic.sda, qwiic.scl); + logger.info("Hidden I2C: SDA={}, SCL={}", hidden_i2c.sda, hidden_i2c.scl); + logger.info("LEDs: user={}, status={}", leds.user, leds.status); + logger.info("Encoder bus: SCLK={}, MISO={}, MOSI={}", enc_bus.sclk, enc_bus.miso, enc_bus.mosi); + logger.info("Encoder CS: [{}, {}, {}, {}]", encoder_cs[0], encoder_cs[1], encoder_cs[2], + encoder_cs[3]); + logger.info("Servo pins: [{}, {}, {}, {}]", servos[0], servos[1], servos[2], servos[3]); + for (size_t i = 0; i < espp::MotorGoPlink::num_motor_channels(); i++) { + auto pins = board.motor_pins(i); + logger.info("Motor {} pins: pwm_a={}, pwm_b={}", i + 1, pins.pwm_a, pins.pwm_b); + } +} + +void log_imu_data(espp::MotorGoPlink &board) { + auto imu = board.imu(); + if (!imu) { + logger.warn("IMU is not initialized"); + return; + } + + static int64_t last_update_us = esp_timer_get_time(); + int64_t now_us = esp_timer_get_time(); + float dt = (now_us - last_update_us) / 1'000'000.0f; + last_update_us = now_us; + + std::error_code ec; + if (!imu->update(dt, ec)) { + logger.warn("Failed to update IMU: {}", ec.message()); + return; + } + + auto accel = imu->get_accelerometer(); + auto gyro = imu->get_gyroscope(); + auto temp = imu->get_temperature(); + logger.info("IMU accel [g]: {:.3f}, {:.3f}, {:.3f} | gyro [dps]: {:.3f}, {:.3f}, {:.3f} | " + "temp [C]: {:.1f}", + accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z, temp); +} + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_ENCODER_POLLING +void log_encoder_angles(espp::MotorGoPlink &board) { + std::array angles{}; + for (size_t i = 0; i < angles.size(); i++) { + auto encoder = board.encoder(i); + if (!encoder) { + logger.warn("Encoder {} is not initialized", i + 1); + return; + } + std::error_code ec; + encoder->update(ec); + if (ec) { + logger.warn("Failed to update encoder {}: {}", i + 1, ec.message()); + return; + } + angles[i] = encoder->get_radians(); + } + + logger.info("Encoder angles [rad]: {:.3f}, {:.3f}, {:.3f}, {:.3f}", angles[0], angles[1], + angles[2], angles[3]); +} +#endif + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP +void update_motor_demo(espp::MotorGoPlink &board, float &phase) { + constexpr float amplitude = 0.20f; + constexpr float phase_step = 0.20f; + constexpr float channel_offset = std::numbers::pi_v / 2.0f; + + for (size_t i = 0; i < espp::MotorGoPlink::num_motor_channels(); i++) { + float speed = amplitude * std::sin(phase + channel_offset * i); + board.set_motor_speed(i, speed); + } + + phase = std::fmod(phase + phase_step, 2.0f * std::numbers::pi_v); +} +#endif +} // namespace + +extern "C" void app_main() { + auto &board = espp::MotorGoPlink::get(); + + if (!board.initialize_leds()) { + logger.warn("Failed to initialize indicator LEDs"); + } else { + board.start_led_breathing(); + } + + if (!board.initialize_motors()) { + logger.error("Failed to initialize motor PWM outputs"); + } else { + board.stop_all_motors(); + } + + if (!board.initialize_imu()) { + logger.warn("Failed to initialize onboard IMU"); + } else { + logger.info("IMU initialized"); + } + + log_board_info(board); + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_ENCODER_POLLING + if (!board.initialize_encoders(false)) { + logger.warn("Failed to initialize encoders"); + } else { + logger.info("Encoder polling enabled"); + } +#else + logger.info("Encoder polling disabled"); +#endif + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP + logger.warn("Motor sweep enabled; ensure the board is connected to a safe test setup"); +#else + logger.info("Motor sweep disabled; all motors remain stopped"); +#endif + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP + float motor_phase = 0.0f; +#endif + while (true) { +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP + update_motor_demo(board, motor_phase); +#endif + +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_ENCODER_POLLING + log_encoder_angles(board); +#endif + + log_imu_data(board); + + vTaskDelay(pdMS_TO_TICKS(500)); + } +} diff --git a/components/motorgo-plink/example/sdkconfig.defaults b/components/motorgo-plink/example/sdkconfig.defaults new file mode 100644 index 000000000..b01fdbeb4 --- /dev/null +++ b/components/motorgo-plink/example/sdkconfig.defaults @@ -0,0 +1 @@ +CONFIG_IDF_TARGET="esp32s3" diff --git a/components/motorgo-plink/idf_component.yml b/components/motorgo-plink/idf_component.yml new file mode 100644 index 000000000..9760239e3 --- /dev/null +++ b/components/motorgo-plink/idf_component.yml @@ -0,0 +1,33 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "MotorGo Plink Board Support Package (BSP) component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/motorgo-plink" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/motorgo_plink.html" +examples: + - path: example +tags: + - cpp + - Component + - ESP32S3 + - MotorGo + - Plink + - BSP + - Motor + - Encoder + - Servo +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/i2c: '>=1.0' + espp/led: '>=1.0' + espp/lsm6dso: '>=1.0' + espp/math: '>=1.0' + espp/mt6701: '>=1.0' + espp/spi: '>=1.0' + espp/task: '>=1.0' +targets: + - esp32s3 diff --git a/components/motorgo-plink/include/motorgo-plink.hpp b/components/motorgo-plink/include/motorgo-plink.hpp new file mode 100644 index 000000000..7338d236d --- /dev/null +++ b/components/motorgo-plink/include/motorgo-plink.hpp @@ -0,0 +1,393 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include + +#include "base_component.hpp" +#include "gaussian.hpp" +#include "i2c.hpp" +#include "led.hpp" +#include "lsm6dso.hpp" +#include "mt6701.hpp" +#include "spi.hpp" +#include "task.hpp" + +namespace espp { +/// MotorGoPlink is a lightweight board-support component for the MotorGo Plink +/// hardware. +/// +/// It exposes the documented pin mapping for the board's: +/// - four dual-PWM DC motor outputs +/// - four encoder chip-selects on a shared SSI/SPI bus +/// - four RC-servo signal pins +/// - one onboard LSM6DS33 IMU on the hidden I2C bus +/// - Qwiic and hidden I2C buses +/// - user and status LEDs +/// +/// The class is a singleton and can be accessed with get(). +/// +/// \section motorgo_plink_ex1 MotorGo Plink Example +/// \snippet motorgo_plink_example.cpp motorgo-plink example +class MotorGoPlink : public BaseComponent { +public: + /// Alias for the SSI-based magnetic encoder helper used on each motor + /// channel. + using Encoder = espp::Mt6701; + /// Alias for the onboard hidden-bus IMU helper. + using Imu = espp::Lsm6dso; + + /// Pin mapping for one DC motor channel. + struct MotorPins { + gpio_num_t pwm_a{GPIO_NUM_NC}; ///< First PWM pin for the motor channel. + gpio_num_t pwm_b{GPIO_NUM_NC}; ///< Second PWM pin for the motor channel. + }; + + /// Shared encoder SSI/SPI bus pins. + struct EncoderBusPins { + gpio_num_t sclk{GPIO_NUM_NC}; ///< Shared encoder clock pin. + gpio_num_t miso{GPIO_NUM_NC}; ///< Shared encoder data pin. + gpio_num_t mosi{GPIO_NUM_NC}; ///< Dummy MOSI pin used to satisfy the SPI host. + }; + + /// I2C pin mapping. + struct I2cPins { + gpio_num_t sda{GPIO_NUM_NC}; ///< I2C SDA pin. + gpio_num_t scl{GPIO_NUM_NC}; ///< I2C SCL pin. + }; + + /// User-visible LED pin mapping. + struct LedPins { + gpio_num_t user{GPIO_NUM_NC}; ///< User LED pin. + gpio_num_t status{GPIO_NUM_NC}; ///< Status LED pin. + }; + + /// Access the singleton board instance. + /// \return Reference to the singleton MotorGoPlink object. + static MotorGoPlink &get() { + static MotorGoPlink instance; + return instance; + } + + /// Deleted copy constructor. + MotorGoPlink(const MotorGoPlink &) = delete; + /// Deleted copy assignment operator. + MotorGoPlink &operator=(const MotorGoPlink &) = delete; + /// Deleted move constructor. + MotorGoPlink(MotorGoPlink &&) = delete; + /// Deleted move assignment operator. + MotorGoPlink &operator=(MotorGoPlink &&) = delete; + + /// Get the external Qwiic I2C bus. + /// \return Reference to the initialized external I2C bus. + I2c &get_external_i2c(); + + /// Get the external Qwiic I2C bus. + /// \return Reference to the initialized external I2C bus. + I2c &qwiic_i2c(); + + /// Get the internal hidden I2C bus. + /// \return Reference to the initialized internal I2C bus. + I2c &hidden_i2c(); + + /// Get the documented motor pin mappings for all four channels. + /// \return Array containing the two PWM pins for each motor + /// channel. + std::array motor_pins() const; + + /// Get one motor channel pin mapping. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return The motor pin mapping for the requested channel, or a default + /// `MotorPins{}` with `GPIO_NUM_NC` entries if the index is invalid. + MotorPins motor_pins(size_t index) const; + + /// Get the shared encoder bus pin mapping. + /// \return The shared SCLK, MISO, and dummy MOSI pins used for the encoder + /// bus. + EncoderBusPins encoder_bus_pins() const; + + /// Get the four encoder chip-select pins. + /// \return Array containing the chip-select pin for each encoder channel. + std::array encoder_cs_pins() const; + + /// Get one encoder chip-select pin. + /// \param index Zero-based encoder index in the range [0, + /// num_motor_channels()). + /// \return The requested encoder chip-select pin, or `GPIO_NUM_NC` if the + /// index is invalid. + gpio_num_t encoder_cs_pin(size_t index) const; + + /// Get the four servo signal pins. + /// \return Array containing the four documented RC-servo signal GPIOs. + std::array servo_pins() const; + + /// Get one servo signal pin. + /// \param index Zero-based servo index in the range [0, num_servo_channels()). + /// \return The requested servo signal pin, or `GPIO_NUM_NC` if the index is + /// invalid. + gpio_num_t servo_pin(size_t index) const; + + /// Get the external Qwiic I2C pin mapping. + /// \return The external Qwiic SDA/SCL pin mapping. + I2cPins qwiic_pins() const; + + /// Get the hidden internal I2C pin mapping. + /// \return The internal SDA/SCL pin mapping. + I2cPins hidden_i2c_pins() const; + + /// Get the user/status LED pins. + /// \return The documented user and status LED GPIOs. + LedPins led_pins() const; + + /// Initialize the eight PWM outputs used by the four motor channels. + /// \param pwm_frequency_hz PWM carrier frequency for all four motor channels. + /// \return True if the motor PWM helper was initialized; + /// false if the PWM frequency is invalid or the motor outputs were + /// already initialized. + bool initialize_motors(size_t pwm_frequency_hz = motor_default_pwm_frequency_hz()); + + /// Set a normalized motor speed in the range [-1, 1]. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \param speed Normalized motor command. Values are clamped to [-1.0, 1.0], + /// with positive values driving `pwm_a`, negative values driving + /// `pwm_b`, and zero disabling both PWM outputs for that motor. + /// \return True if the motor command was applied; false if the motor PWM + /// subsystem has not been initialized or the index is invalid. + bool set_motor_speed(size_t index, float speed); + + /// Get the last commanded normalized motor speed. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return The last commanded normalized speed for that channel, or 0.0f if + /// the index is invalid. + float motor_speed(size_t index) const; + + /// Stop one motor channel. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + void stop_motor(size_t index); + + /// Stop all motor channels. + /// \details If the motor PWM subsystem has been initialized, each motor is + /// actively commanded to zero speed. Otherwise the cached command + /// values are simply reset to zero. + void stop_all_motors(); + + /// Get the LEDC wrapper used for the eight motor PWM outputs. + /// \return Shared pointer to the motor PWM helper, or `nullptr` if + /// initialize_motors() has not been called yet. + std::shared_ptr motor_pwm(); + + /// Initialize the shared encoder bus and create the four MT6701 SSI helpers. + /// \param run_tasks If true, each encoder starts its own update task after + /// initialization. If false, callers must invoke + /// `Encoder::update()` manually. + /// \return True if the shared bus and all four encoder helpers were + /// initialized successfully; false otherwise. + bool initialize_encoders(bool run_tasks = true); + + /// Get one encoder helper. + /// \param index Zero-based encoder index in the range [0, + /// num_motor_channels()). + /// \return Shared pointer to the requested encoder helper, or `nullptr` if the + /// index is invalid or that encoder has not been initialized yet. + std::shared_ptr encoder(size_t index); + + /// Reset one encoder's accumulator. + /// \param index Zero-based encoder index in the range [0, + /// num_motor_channels()). + /// \details If the encoder has not been initialized, the request is ignored + /// after logging an error. + void reset_encoder_accumulator(size_t index); + + /// Initialize the onboard IMU on the hidden I2C bus. + /// \param orientation_filter Optional orientation filter callback used by the + /// IMU driver when update() is called. + /// \param imu_config Basic accelerometer / gyroscope configuration to apply + /// during initialization. + /// \return True if the hidden-bus I2C device and IMU helper were initialized + /// successfully; false otherwise. + bool initialize_imu(const Imu::filter_fn &orientation_filter = nullptr, + const Imu::ImuConfig &imu_config = { + .accel_range = Imu::AccelRange::RANGE_2G, + .accel_odr = Imu::AccelODR::ODR_104_HZ, + .gyro_range = Imu::GyroRange::DPS_1000, + .gyro_odr = Imu::GyroODR::ODR_104_HZ, + }); + + /// Get the onboard IMU helper. + /// \return Shared pointer to the initialized IMU helper, or `nullptr` if + /// initialize_imu() has not been called yet. + std::shared_ptr imu() const; + + /// Initialize the user/status LED helpers. + /// \param breathing_period Default breathing period in seconds for + /// start_led_breathing(). + /// \return True if the indicator LED PWM helper and breathing task were + /// created successfully; false if they were already initialized. + bool initialize_leds(float breathing_period = 3.5f); + + /// Start breathing both indicator LEDs. + /// \details This uses the shared Gaussian waveform returned by gaussian(). + void start_led_breathing(); + + /// Stop breathing and turn both indicator LEDs off. + void stop_led_breathing(); + + /// Set the user LED brightness in the range [0, 1]. + /// \param brightness Desired brightness, clamped to [0.0, 1.0]. + /// \return True if the user LED duty cycle was updated; false if the LEDs have + /// not been initialized or the breathing task is currently running. + bool set_user_led_brightness(float brightness); + + /// Get the user LED brightness in the range [0, 1]. + /// \return Current user LED brightness normalized to [0.0, 1.0], or 0.0f if + /// the LEDs have not been initialized or the duty cycle could not be + /// read. + float get_user_led_brightness(); + + /// Set the status LED brightness in the range [0, 1]. + /// \param brightness Desired brightness, clamped to [0.0, 1.0]. + /// \return True if the status LED duty cycle was updated; false if the LEDs + /// have not been initialized or the breathing task is currently + /// running. + bool set_status_led_brightness(float brightness); + + /// Get the status LED brightness in the range [0, 1]. + /// \return Current status LED brightness normalized to [0.0, 1.0], or 0.0f if + /// the LEDs have not been initialized or the duty cycle could not be + /// read. + float get_status_led_brightness(); + + /// Set the LED breathing period in seconds. + /// \param period Breathing period in seconds. Must be greater than zero. + /// \return True if the period was accepted; false if the value is not + /// positive. + bool set_led_breathing_period(float period); + + /// Get the LED breathing period in seconds. + /// \return The configured LED breathing period in seconds. + float get_led_breathing_period(); + + /// Get the LED helper used for the indicator LEDs. + /// \return Shared pointer to the indicator LED helper, or `nullptr` if + /// initialize_leds() has not been called yet. + std::shared_ptr leds(); + + /// Get the Gaussian waveform used for LED breathing. + /// \return Reference to the Gaussian waveform object used by the LED + /// breathing task. + espp::Gaussian &gaussian(); + + /// Get the number of supported motor channels. + /// \return The number of motor channels exposed by the board. + static constexpr size_t num_motor_channels() { return motor_pin_map_.size(); } + /// Get the number of exposed servo signal pins. + /// \return The number of servo channels exposed by the board. + static constexpr size_t num_servo_channels() { return servo_pin_map_.size(); } + /// Get the default PWM frequency used for motor outputs. + /// \return Default motor PWM frequency in Hz. + static constexpr size_t motor_default_pwm_frequency_hz() { + return motor_default_pwm_frequency_hz_; + } + /// Get the encoder bus clock speed. + /// \return Default encoder SSI/SPI clock speed in Hz. + static constexpr size_t encoder_spi_clock_speed_hz() { return encoder_spi_clock_speed_hz_; } + /// Get the user LED GPIO. + /// \return User LED pin. + static constexpr gpio_num_t user_led_pin() { return user_led_pin_; } + /// Get the status LED GPIO. + /// \return Status LED pin. + static constexpr gpio_num_t status_led_pin() { return status_led_pin_; } + +protected: + MotorGoPlink(); + + bool initialize_encoder_spi(); + bool read_encoder(size_t index, uint8_t *data, size_t size); + float led_breathe(); + bool led_task_callback(std::mutex &m, std::condition_variable &cv, bool &task_notified); + + static constexpr std::array motor_pin_map_{{ + {.pwm_a = GPIO_NUM_16, .pwm_b = GPIO_NUM_15}, + {.pwm_a = GPIO_NUM_17, .pwm_b = GPIO_NUM_18}, + {.pwm_a = GPIO_NUM_8, .pwm_b = GPIO_NUM_3}, + {.pwm_a = GPIO_NUM_11, .pwm_b = GPIO_NUM_12}, + }}; + static constexpr std::array encoder_cs_pin_map_{ + GPIO_NUM_4, + GPIO_NUM_5, + GPIO_NUM_10, + GPIO_NUM_9, + }; + static constexpr std::array servo_pin_map_{ + GPIO_NUM_38, + GPIO_NUM_40, + GPIO_NUM_39, + GPIO_NUM_37, + }; + static constexpr EncoderBusPins encoder_bus_pin_map_{ + .sclk = GPIO_NUM_7, + .miso = GPIO_NUM_6, + .mosi = GPIO_NUM_45, + }; + static constexpr I2cPins qwiic_pin_map_{ + .sda = GPIO_NUM_1, + .scl = GPIO_NUM_2, + }; + static constexpr I2cPins hidden_i2c_pin_map_{ + .sda = GPIO_NUM_13, + .scl = GPIO_NUM_14, + }; + static constexpr gpio_num_t user_led_pin_ = GPIO_NUM_42; + static constexpr gpio_num_t status_led_pin_ = GPIO_NUM_41; + static constexpr spi_host_device_t encoder_spi_host_ = SPI2_HOST; + static constexpr size_t encoder_spi_clock_speed_hz_ = 10 * 1000 * 1000; + static constexpr size_t motor_default_pwm_frequency_hz_ = 20 * 1000; + static constexpr size_t indicator_led_pwm_frequency_hz_ = 5 * 1000; + static constexpr float encoder_update_period_seconds_ = 0.001f; + + I2c qwiic_i2c_{{.port = I2C_NUM_0, + .sda_io_num = qwiic_pin_map_.sda, + .scl_io_num = qwiic_pin_map_.scl, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE}}; + I2c hidden_i2c_{{.port = I2C_NUM_1, + .sda_io_num = hidden_i2c_pin_map_.sda, + .scl_io_num = hidden_i2c_pin_map_.scl, + .sda_pullup_en = GPIO_PULLUP_ENABLE, + .scl_pullup_en = GPIO_PULLUP_ENABLE}}; + + std::unique_ptr encoder_spi_; + std::array, 4> encoder_spi_devices_{}; + std::array, 4> encoders_{}; + std::shared_ptr> imu_i2c_device_; + std::shared_ptr imu_; + + std::array motor_channels_{{ + {static_cast(motor_pin_map_[0].pwm_a), LEDC_CHANNEL_0, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[0].pwm_b), LEDC_CHANNEL_1, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[1].pwm_a), LEDC_CHANNEL_2, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[1].pwm_b), LEDC_CHANNEL_3, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[2].pwm_a), LEDC_CHANNEL_4, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[2].pwm_b), LEDC_CHANNEL_5, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[3].pwm_a), LEDC_CHANNEL_6, LEDC_TIMER_0}, + {static_cast(motor_pin_map_[3].pwm_b), LEDC_CHANNEL_7, LEDC_TIMER_0}, + }}; + std::array led_channels_{{ + {static_cast(user_led_pin_), LEDC_CHANNEL_4, LEDC_TIMER_1}, + {static_cast(status_led_pin_), LEDC_CHANNEL_5, LEDC_TIMER_1}, + }}; + + std::shared_ptr motor_pwm_; + std::shared_ptr indicator_leds_; + std::array motor_speeds_{0.0f, 0.0f, 0.0f, 0.0f}; + std::unique_ptr led_task_; + std::atomic breathing_period_{3.5f}; + std::chrono::high_resolution_clock::time_point breathing_start_{}; + espp::Gaussian gaussian_{{.gamma = 0.1f, .alpha = 1.0f, .beta = 0.5f}}; +}; // class MotorGoPlink +} // namespace espp diff --git a/components/motorgo-plink/src/motorgo-plink.cpp b/components/motorgo-plink/src/motorgo-plink.cpp new file mode 100644 index 000000000..c01bddb51 --- /dev/null +++ b/components/motorgo-plink/src/motorgo-plink.cpp @@ -0,0 +1,381 @@ +#include "motorgo-plink.hpp" + +#include +#include + +using namespace espp; + +MotorGoPlink::MotorGoPlink() + : BaseComponent("MotorGoPlink") {} + +I2c &MotorGoPlink::get_external_i2c() { return qwiic_i2c_; } + +I2c &MotorGoPlink::qwiic_i2c() { return qwiic_i2c_; } + +I2c &MotorGoPlink::hidden_i2c() { return hidden_i2c_; } + +std::array MotorGoPlink::motor_pins() const { return motor_pin_map_; } + +MotorGoPlink::MotorPins MotorGoPlink::motor_pins(size_t index) const { + if (index >= motor_pin_map_.size()) { + logger_.error("Invalid motor index: {}", index); + return {}; + } + return motor_pin_map_[index]; +} + +MotorGoPlink::EncoderBusPins MotorGoPlink::encoder_bus_pins() const { return encoder_bus_pin_map_; } + +std::array MotorGoPlink::encoder_cs_pins() const { return encoder_cs_pin_map_; } + +gpio_num_t MotorGoPlink::encoder_cs_pin(size_t index) const { + if (index >= encoder_cs_pin_map_.size()) { + logger_.error("Invalid encoder index: {}", index); + return GPIO_NUM_NC; + } + return encoder_cs_pin_map_[index]; +} + +std::array MotorGoPlink::servo_pins() const { return servo_pin_map_; } + +gpio_num_t MotorGoPlink::servo_pin(size_t index) const { + if (index >= servo_pin_map_.size()) { + logger_.error("Invalid servo index: {}", index); + return GPIO_NUM_NC; + } + return servo_pin_map_[index]; +} + +MotorGoPlink::I2cPins MotorGoPlink::qwiic_pins() const { return qwiic_pin_map_; } + +MotorGoPlink::I2cPins MotorGoPlink::hidden_i2c_pins() const { return hidden_i2c_pin_map_; } + +MotorGoPlink::LedPins MotorGoPlink::led_pins() const { + return {.user = user_led_pin_, .status = status_led_pin_}; +} + +bool MotorGoPlink::initialize_motors(size_t pwm_frequency_hz) { + if (motor_pwm_) { + logger_.error("Motor PWM already initialized"); + return false; + } + if (pwm_frequency_hz == 0) { + logger_.error("Motor PWM frequency must be greater than zero"); + return false; + } + motor_pwm_ = std::make_shared(espp::Led::Config{ + .timer = LEDC_TIMER_0, + .frequency_hz = pwm_frequency_hz, + .channels = + std::vector(motor_channels_.begin(), motor_channels_.end()), + .duty_resolution = LEDC_TIMER_13_BIT, + .log_level = get_log_level(), + }); + stop_all_motors(); + return true; +} + +bool MotorGoPlink::set_motor_speed(size_t index, float speed) { + if (!motor_pwm_) { + logger_.error("Motor PWM not initialized"); + return false; + } + if (index >= motor_pin_map_.size()) { + logger_.error("Invalid motor index: {}", index); + return false; + } + speed = std::clamp(speed, -1.0f, 1.0f); + float duty = 100.0f * std::abs(speed); + size_t pwm_a_index = index * 2; + size_t pwm_b_index = pwm_a_index + 1; + + bool ok_a = true; + bool ok_b = true; + if (speed > 0.0f) { + ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, 0.0f); + ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, duty); + } else if (speed < 0.0f) { + ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, 0.0f); + ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, duty); + } else { + ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, 0.0f); + ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, 0.0f); + } + bool ok = ok_a && ok_b; + if (ok) { + motor_speeds_[index] = speed; + } + return ok; +} + +float MotorGoPlink::motor_speed(size_t index) const { + if (index >= motor_speeds_.size()) { + logger_.error("Invalid motor index: {}", index); + return 0.0f; + } + return motor_speeds_[index]; +} + +void MotorGoPlink::stop_motor(size_t index) { set_motor_speed(index, 0.0f); } + +void MotorGoPlink::stop_all_motors() { + if (!motor_pwm_) { + motor_speeds_.fill(0.0f); + return; + } + for (size_t i = 0; i < motor_pin_map_.size(); i++) { + set_motor_speed(i, 0.0f); + } +} + +std::shared_ptr MotorGoPlink::motor_pwm() { return motor_pwm_; } + +bool MotorGoPlink::initialize_encoders(bool run_tasks) { + if (!initialize_encoder_spi()) { + return false; + } + for (size_t i = 0; i < encoders_.size(); i++) { + if (encoders_[i]) { + continue; + } + Encoder::Config config{ + .read = [this, i](uint8_t *data, size_t size) -> bool { + return read_encoder(i, data, size); + }, + .update_period = std::chrono::duration(encoder_update_period_seconds_), + .auto_init = false, + .run_task = false, + .log_level = get_log_level(), + }; + auto encoder = std::make_shared(config); + std::error_code ec; + encoder->initialize(run_tasks, ec); + if (ec) { + logger_.error("Failed to initialize encoder {}: {}", i + 1, ec.message()); + return false; + } + encoders_[i] = encoder; + } + return true; +} + +std::shared_ptr MotorGoPlink::encoder(size_t index) { + if (index >= encoders_.size()) { + logger_.error("Invalid encoder index: {}", index); + return nullptr; + } + return encoders_[index]; +} + +void MotorGoPlink::reset_encoder_accumulator(size_t index) { + auto encoder_ptr = encoder(index); + if (!encoder_ptr) { + logger_.error("Encoder {} not initialized", index + 1); + return; + } + encoder_ptr->reset_accumulator(); +} + +bool MotorGoPlink::initialize_imu(const Imu::filter_fn &orientation_filter, + const Imu::ImuConfig &imu_config) { + if (imu_) { + logger_.warn("IMU already initialized, not initializing again!"); + return false; + } + + std::error_code ec; + imu_i2c_device_ = hidden_i2c_.add_device( + { + .device_address = Imu::DEFAULT_I2C_ADDRESS, + .timeout_ms = static_cast(hidden_i2c_.config().timeout_ms), + .scl_speed_hz = hidden_i2c_.config().clk_speed, + .log_level = espp::Logger::Verbosity::WARN, + }, + ec); + if (!imu_i2c_device_) { + logger_.error("Could not initialize IMU I2C device: {}", ec.message()); + return false; + } + + Imu::Config config{ + .device_address = Imu::DEFAULT_I2C_ADDRESS, + .write = espp::make_i2c_addressed_write(imu_i2c_device_), + .read = espp::make_i2c_addressed_read(imu_i2c_device_), + .imu_config = imu_config, + .orientation_filter = orientation_filter, + .auto_init = true, + .log_level = get_log_level(), + }; + + logger_.info("Initializing hidden-bus IMU"); + imu_ = std::make_shared(config); + return true; +} + +std::shared_ptr MotorGoPlink::imu() const { return imu_; } + +bool MotorGoPlink::initialize_leds(float breathing_period) { + if (indicator_leds_) { + logger_.error("Indicator LEDs already initialized"); + return false; + } + indicator_leds_ = std::make_shared(espp::Led::Config{ + .timer = LEDC_TIMER_1, + .frequency_hz = indicator_led_pwm_frequency_hz_, + .channels = std::vector(led_channels_.begin(), led_channels_.end()), + .duty_resolution = LEDC_TIMER_10_BIT, + .log_level = get_log_level(), + }); + using namespace std::placeholders; + led_task_ = espp::Task::make_unique( + {.callback = std::bind(&MotorGoPlink::led_task_callback, this, _1, _2, _3), + .task_config = {.name = "motorgo_plink_led"}}); + set_led_breathing_period(breathing_period); + set_user_led_brightness(0.0f); + set_status_led_brightness(0.0f); + return true; +} + +void MotorGoPlink::start_led_breathing() { + if (!indicator_leds_ || !led_task_) { + logger_.error("Indicator LEDs not initialized"); + return; + } + breathing_start_ = std::chrono::high_resolution_clock::now(); + led_task_->start(); +} + +void MotorGoPlink::stop_led_breathing() { + if (!indicator_leds_ || !led_task_) { + logger_.error("Indicator LEDs not initialized"); + return; + } + led_task_->stop(); + indicator_leds_->set_duty(led_channels_[0].channel, 0.0f); + indicator_leds_->set_duty(led_channels_[1].channel, 0.0f); +} + +bool MotorGoPlink::set_user_led_brightness(float brightness) { + if (!indicator_leds_ || !led_task_) { + logger_.error("Indicator LEDs not initialized"); + return false; + } + if (led_task_->is_running()) { + logger_.error("Cannot set user LED brightness while breathing"); + return false; + } + brightness = std::clamp(brightness, 0.0f, 1.0f); + return indicator_leds_->set_duty(led_channels_[0].channel, 100.0f * brightness); +} + +float MotorGoPlink::get_user_led_brightness() { + if (!indicator_leds_) { + logger_.error("Indicator LEDs not initialized"); + return 0.0f; + } + auto maybe_duty = indicator_leds_->get_duty(led_channels_[0].channel); + if (!maybe_duty.has_value()) { + logger_.error("Failed to get user LED duty"); + return 0.0f; + } + return maybe_duty.value() / 100.0f; +} + +bool MotorGoPlink::set_status_led_brightness(float brightness) { + if (!indicator_leds_ || !led_task_) { + logger_.error("Indicator LEDs not initialized"); + return false; + } + if (led_task_->is_running()) { + logger_.error("Cannot set status LED brightness while breathing"); + return false; + } + brightness = std::clamp(brightness, 0.0f, 1.0f); + return indicator_leds_->set_duty(led_channels_[1].channel, 100.0f * brightness); +} + +float MotorGoPlink::get_status_led_brightness() { + if (!indicator_leds_) { + logger_.error("Indicator LEDs not initialized"); + return 0.0f; + } + auto maybe_duty = indicator_leds_->get_duty(led_channels_[1].channel); + if (!maybe_duty.has_value()) { + logger_.error("Failed to get status LED duty"); + return 0.0f; + } + return maybe_duty.value() / 100.0f; +} + +bool MotorGoPlink::set_led_breathing_period(float period) { + if (period <= 0.0f) { + logger_.error("Invalid LED breathing period: {}", period); + return false; + } + breathing_period_ = period; + return true; +} + +float MotorGoPlink::get_led_breathing_period() { return breathing_period_; } + +std::shared_ptr MotorGoPlink::leds() { return indicator_leds_; } + +espp::Gaussian &MotorGoPlink::gaussian() { return gaussian_; } + +bool MotorGoPlink::initialize_encoder_spi() { + if (encoder_spi_) { + return true; + } + encoder_spi_ = std::make_unique(Spi::Config{ + .host = encoder_spi_host_, + .sclk_io_num = encoder_bus_pin_map_.sclk, + .mosi_io_num = encoder_bus_pin_map_.mosi, + .miso_io_num = encoder_bus_pin_map_.miso, + .max_transfer_sz = 32, + .log_level = get_log_level(), + }); + + std::error_code ec; + for (size_t i = 0; i < encoder_spi_devices_.size(); i++) { + encoder_spi_devices_[i] = encoder_spi_->add_device( + Spi::DeviceConfig{ + .mode = 0, + .clock_speed_hz = encoder_spi_clock_speed_hz_, + .cs_io_num = encoder_cs_pin_map_[i], + .queue_size = 1, + }, + ec); + if (ec || !encoder_spi_devices_[i]) { + logger_.error("Failed to initialize encoder {} SPI device: {}", i + 1, ec.message()); + return false; + } + } + return true; +} + +bool IRAM_ATTR MotorGoPlink::read_encoder(size_t index, uint8_t *data, size_t size) { + if (index >= encoder_spi_devices_.size() || !encoder_spi_devices_[index]) { + return false; + } + std::error_code ec; + return encoder_spi_devices_[index]->read(std::span(data, size), {}, ec); +} + +float MotorGoPlink::led_breathe() { + auto now = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::duration(now - breathing_start_).count(); + float t = std::fmod(elapsed, breathing_period_) / breathing_period_; + return gaussian_(t); +} + +bool MotorGoPlink::led_task_callback(std::mutex &m, std::condition_variable &cv, + bool &task_notified) { + using namespace std::chrono_literals; + float brightness = led_breathe(); + indicator_leds_->set_duty(led_channels_[0].channel, 100.0f * brightness); + indicator_leds_->set_duty(led_channels_[1].channel, 100.0f * brightness); + std::unique_lock lk(m); + cv.wait_for(lk, 10ms, [&task_notified] { return task_notified; }); + task_notified = false; + return false; +} diff --git a/doc/en/imu/lsm6dso.rst b/doc/en/imu/lsm6dso.rst index b125dbfe7..194738957 100644 --- a/doc/en/imu/lsm6dso.rst +++ b/doc/en/imu/lsm6dso.rst @@ -1,8 +1,13 @@ LSM6DSO 6-Axis IMU ****************** -The `Lsm6dso` component provides a driver for the LSM6DSO 6-Axis -Inertial Measurement Unit (IMU) from STMicroelectronics. +The `Lsm6dso` component provides a driver for the LSM6DSO 6-Axis Inertial +Measurement Unit (IMU) from STMicroelectronics and compatible LSM6DS-family +parts that share the same basic accel / gyro register map. + +For basic accel / gyro operation, the driver also accepts the LSM6DS33 +``WHO_AM_I`` value (`0x69`), allowing boards such as MotorGo Plink to reuse the +same component over I2C without a separate IMU driver. .. ------------------------------- Example ------------------------------------- diff --git a/doc/en/motorgo_plink.rst b/doc/en/motorgo_plink.rst new file mode 100644 index 000000000..253927f89 --- /dev/null +++ b/doc/en/motorgo_plink.rst @@ -0,0 +1,37 @@ +MotorGo Plink +************* + +MotorGo Plink +------------- + +The MotorGo Plink is a four-channel ESP32-S3 motor-control board from Every +Flavor Robotics. The `espp::MotorGoPlink` component provides a singleton board +abstraction for the documented pin mapping of its DC motor outputs, encoder +chip-selects, servo signal header, onboard LSM6DS33 IMU, Qwiic and internal I2C +buses, and user/status LEDs. + +It also provides helper methods for: + +- initializing the four dual-PWM motor channels and commanding normalized motor + speeds, where positive commands drive ``pwm_a`` and negative commands drive + ``pwm_b`` +- initializing the shared SSI/SPI encoder bus and creating four + `espp::Mt6701` helpers +- initializing the onboard LSM6DS33 IMU on the hidden I2C bus via the + `espp::Lsm6dso` helper +- accessing the four RC-servo signal pins for use with an external servo driver +- controlling the user and status LEDs with direct brightness setters or a + Gaussian breathing effect + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + motorgo_plink_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/motorgo-plink.inc diff --git a/doc/en/motorgo_plink_example.md b/doc/en/motorgo_plink_example.md new file mode 100644 index 000000000..c0d777751 --- /dev/null +++ b/doc/en/motorgo_plink_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/motorgo-plink/example/README.md +``` From a336a0647212851d516609e0320e3de5cc0e92a5 Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 8 Jun 2026 15:51:27 -0500 Subject: [PATCH 2/4] update CI; add bdc driver component, update motorgo plink to work better and use new bdc driver component rather than using ledc for motors and leds --- .github/workflows/build.yml | 4 + .github/workflows/upload_components.yml | 2 + components/bdc_driver/CMakeLists.txt | 4 + components/bdc_driver/README.md | 23 ++ components/bdc_driver/example/CMakeLists.txt | 21 ++ components/bdc_driver/example/README.md | 28 ++ .../bdc_driver/example/main/CMakeLists.txt | 2 + .../example/main/bdc_driver_example.cpp | 47 +++ .../bdc_driver/example/sdkconfig.defaults | 1 + components/bdc_driver/idf_component.yml | 21 ++ components/bdc_driver/include/bdc_driver.hpp | 145 ++++++++ components/bdc_driver/src/bdc_driver.cpp | 342 ++++++++++++++++++ components/motorgo-plink/CMakeLists.txt | 2 +- components/motorgo-plink/README.md | 8 +- components/motorgo-plink/example/README.md | 11 +- .../example/main/motorgo_plink_example.cpp | 46 ++- components/motorgo-plink/idf_component.yml | 1 + .../motorgo-plink/include/motorgo-plink.hpp | 37 +- .../motorgo-plink/src/motorgo-plink.cpp | 114 +++--- doc/Doxyfile | 4 + doc/en/bldc/bdc_driver.rst | 23 ++ doc/en/bldc/bdc_driver_example.md | 2 + doc/en/bldc/index.rst | 16 +- doc/en/index.rst | 1 + doc/en/motorgo_plink.rst | 6 +- 25 files changed, 821 insertions(+), 90 deletions(-) create mode 100644 components/bdc_driver/CMakeLists.txt create mode 100644 components/bdc_driver/README.md create mode 100644 components/bdc_driver/example/CMakeLists.txt create mode 100644 components/bdc_driver/example/README.md create mode 100644 components/bdc_driver/example/main/CMakeLists.txt create mode 100644 components/bdc_driver/example/main/bdc_driver_example.cpp create mode 100644 components/bdc_driver/example/sdkconfig.defaults create mode 100644 components/bdc_driver/idf_component.yml create mode 100644 components/bdc_driver/include/bdc_driver.hpp create mode 100644 components/bdc_driver/src/bdc_driver.cpp create mode 100644 doc/en/bldc/bdc_driver.rst create mode 100644 doc/en/bldc/bdc_driver_example.md diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 19b55bd5c..c39c0f5bf 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -33,6 +33,8 @@ jobs: target: esp32s3 - path: 'components/aw9523/example' target: esp32 + - path: 'components/bdc_driver/example' + target: esp32s3 - path: 'components/binary-log/example' target: esp32s3 - path: 'components/bldc_haptics/example' @@ -141,6 +143,8 @@ jobs: target: esp32 - path: 'components/motorgo-mini/example' target: esp32s3 + - path: 'components/motorgo-plink/example' + target: esp32s3 - path: 'components/mcp23x17/example' target: esp32s3 - path: 'components/mt6701/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 05a481f7c..6472b731c 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -36,6 +36,7 @@ jobs: components/aw9523 components/base_component components/base_peripheral + components/bdc_driver components/binary-log components/bldc_driver components/bldc_haptics @@ -92,6 +93,7 @@ jobs: components/mcp23x17 components/monitor components/motorgo-mini + components/motorgo-plink components/mt6701 components/ndef components/neopixel diff --git a/components/bdc_driver/CMakeLists.txt b/components/bdc_driver/CMakeLists.txt new file mode 100644 index 000000000..7f81a300f --- /dev/null +++ b/components/bdc_driver/CMakeLists.txt @@ -0,0 +1,4 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component esp_driver_gpio esp_driver_mcpwm) diff --git a/components/bdc_driver/README.md b/components/bdc_driver/README.md new file mode 100644 index 000000000..277f25fdc --- /dev/null +++ b/components/bdc_driver/README.md @@ -0,0 +1,23 @@ +# BDC (Brushed DC) Motor Driver Component + +[![Badge](https://components.espressif.com/components/espp/bdc_driver/badge.svg)](https://components.espressif.com/components/espp/bdc_driver) + +The `BdcDriver` component wraps the `ESP MCPWM Peripheral +`_ +to drive a brushed DC motor through a dual-PWM H-bridge style interface. + +It provides: + +- two MCPWM outputs per motor channel +- direct duty-cycle control for each output +- a signed speed helper where positive commands drive output A, negative + commands drive output B, and zero disables both outputs +- optional driver-enable GPIO handling +- readback of the last commanded normalized and raw duty values for logging and + debugging + +## Example + +The [example](./example) shows a simple sinusoidal speed sweep using two GPIOs. +Update the GPIO assignments in the example to match your hardware before +running it on a real motor driver. diff --git a/components/bdc_driver/example/CMakeLists.txt b/components/bdc_driver/example/CMakeLists.txt new file mode 100644 index 000000000..34eae59a1 --- /dev/null +++ b/components/bdc_driver/example/CMakeLists.txt @@ -0,0 +1,21 @@ +# The following lines of boilerplate have to be in your project's CMakeLists +# in this exact order for cmake to work correctly +cmake_minimum_required(VERSION 3.20) + +set(ENV{IDF_COMPONENT_MANAGER} "0") +include($ENV{IDF_PATH}/tools/cmake/project.cmake) + +set(EXTRA_COMPONENT_DIRS + "../../../components/" + ) + +set( + COMPONENTS + "main bdc_driver logger" + CACHE STRING + "List of components to include" + ) + +project(bdc_driver_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/bdc_driver/example/README.md b/components/bdc_driver/example/README.md new file mode 100644 index 000000000..0f385095e --- /dev/null +++ b/components/bdc_driver/example/README.md @@ -0,0 +1,28 @@ +# BDC Driver Example + +This example shows how to use the `espp::BdcDriver` component to drive a brushed +DC motor through a dual-PWM motor driver. + +## How to use example + +### Hardware Required + +This example requires a brushed DC motor driver or H-bridge that accepts two PWM +inputs for a single motor channel. + +Update the `motor_gpio_a` and `motor_gpio_b` constants in +`example/main/bdc_driver_example.cpp` to match your hardware before flashing the +example. + +### Build and Flash + +Build the project and flash it to the board, then run monitor tool to view +serial output: + +```bash +idf.py -p PORT flash monitor +``` + +(Replace PORT with the name of the serial port to use.) + +(To exit the serial monitor, type `Ctrl-]`.) diff --git a/components/bdc_driver/example/main/CMakeLists.txt b/components/bdc_driver/example/main/CMakeLists.txt new file mode 100644 index 000000000..a941e22ba --- /dev/null +++ b/components/bdc_driver/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/bdc_driver/example/main/bdc_driver_example.cpp b/components/bdc_driver/example/main/bdc_driver_example.cpp new file mode 100644 index 000000000..98bc5fb5a --- /dev/null +++ b/components/bdc_driver/example/main/bdc_driver_example.cpp @@ -0,0 +1,47 @@ +#include +#include + +#include +#include + +#include "bdc_driver.hpp" +#include "logger.hpp" + +namespace { +static auto logger = espp::Logger({ + .tag = "BdcDriver Example", + .level = espp::Logger::Verbosity::INFO, +}); +} // namespace + +extern "C" void app_main() { + //! [bdc_driver example] + constexpr gpio_num_t motor_gpio_a = GPIO_NUM_16; + constexpr gpio_num_t motor_gpio_b = GPIO_NUM_17; + + espp::BdcDriver driver({ + .gpio_a = motor_gpio_a, + .gpio_b = motor_gpio_b, + .group_id = 0, + .log_level = espp::Logger::Verbosity::INFO, + }); + + if (!driver.initialized()) { + logger.error("Failed to initialize BDC driver"); + return; + } + + float phase = 0.0f; + while (true) { + float command = 0.8f * std::sin(phase); + driver.set_speed(command); + auto duty = driver.duty_cycle(); + auto raw = driver.raw_duty(); + logger.info("cmd={:.3f} | pwm_a duty={:.1f}% raw={}/{} | pwm_b duty={:.1f}% raw={}/{}", command, + duty[0] * 100.0f, raw[0], driver.max_raw_duty(), duty[1] * 100.0f, raw[1], + driver.max_raw_duty()); + phase = std::fmod(phase + 0.15f, 2.0f * std::numbers::pi_v); + vTaskDelay(pdMS_TO_TICKS(500)); + } + //! [bdc_driver example] +} diff --git a/components/bdc_driver/example/sdkconfig.defaults b/components/bdc_driver/example/sdkconfig.defaults new file mode 100644 index 000000000..b01fdbeb4 --- /dev/null +++ b/components/bdc_driver/example/sdkconfig.defaults @@ -0,0 +1 @@ +CONFIG_IDF_TARGET="esp32s3" diff --git a/components/bdc_driver/idf_component.yml b/components/bdc_driver/idf_component.yml new file mode 100644 index 000000000..1322bd9cb --- /dev/null +++ b/components/bdc_driver/idf_component.yml @@ -0,0 +1,21 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "BDC (Brushed DC) motor driver component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/bdc_driver" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/bldc/bdc_driver.html" +examples: + - path: example +tags: + - cpp + - Component + - BDC + - Motor + - Driver + - MCPWM +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' diff --git a/components/bdc_driver/include/bdc_driver.hpp b/components/bdc_driver/include/bdc_driver.hpp new file mode 100644 index 000000000..b419595bf --- /dev/null +++ b/components/bdc_driver/include/bdc_driver.hpp @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include "base_component.hpp" + +namespace espp { +/// Driver for brushed DC motors controlled by two PWM outputs. +/// +/// The component uses one MCPWM operator per motor channel and is intended for +/// H-bridge style drivers that accept two PWM inputs. Positive signed speed +/// commands drive output A, negative commands drive output B, and zero disables +/// both outputs. +/// +/// \section bdc_driver_example Example +/// \snippet bdc_driver_example.cpp bdc_driver example +class BdcDriver : public BaseComponent { +public: + /// Default MCPWM timer resolution used by the driver. + static constexpr size_t DEFAULT_TIMER_RESOLUTION_HZ = 80 * 1000 * 1000; + /// Default PWM carrier frequency used by the driver. + static constexpr size_t DEFAULT_PWM_FREQUENCY_HZ = 20 * 1000; + + /// Configuration for one brushed DC motor driver. + struct Config { + gpio_num_t gpio_a{GPIO_NUM_NC}; ///< PWM GPIO for forward/output A. + gpio_num_t gpio_b{GPIO_NUM_NC}; ///< PWM GPIO for reverse/output B. + gpio_num_t gpio_enable{GPIO_NUM_NC}; ///< Optional active-high enable pin. + int group_id{0}; ///< MCPWM group to allocate the timer and operator from. + size_t timer_resolution_hz{ + DEFAULT_TIMER_RESOLUTION_HZ}; ///< MCPWM timer clock resolution in Hz. + size_t pwm_frequency_hz{DEFAULT_PWM_FREQUENCY_HZ}; ///< PWM carrier frequency in Hz. + bool invert_output_a{false}; ///< True to invert output A at the GPIO matrix. + bool invert_output_b{false}; ///< True to invert output B at the GPIO matrix. + Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Component log verbosity. + }; + + /// Construct the brushed DC motor driver. + /// \param config Driver configuration. + explicit BdcDriver(const Config &config); + + /// Destroy the driver and release all MCPWM resources. + ~BdcDriver(); + + /// Check whether initialization completed successfully. + /// \return True if the MCPWM resources were created and the outputs were + /// initialized; false otherwise. + bool initialized() const; + + /// Enable the motor driver output. + /// \return True if the timer started successfully and the optional enable pin + /// was asserted; false otherwise. + bool enable(); + + /// Disable the motor driver output. + /// \return True if the outputs were forced low and the timer was stopped; + /// false otherwise. + bool disable(); + + /// Check whether the driver is currently enabled. + /// \return True if the timer is running; false otherwise. + bool is_enabled() const; + + /// Set the normalized duty cycles for the two driver outputs. + /// \param duty_a Duty cycle for output A in the range [0.0, 1.0]. + /// \param duty_b Duty cycle for output B in the range [0.0, 1.0]. + /// \return True if both outputs were updated successfully; false otherwise. + bool set_duty(float duty_a, float duty_b); + + /// Set a signed normalized motor command. + /// \param speed Signed speed command in the range [-1.0, 1.0]. Positive + /// values drive output A, negative values drive output B, and + /// zero disables both outputs. + /// \return True if the outputs were updated successfully; false otherwise. + bool set_speed(float speed); + + /// Stop the motor by disabling both PWM outputs. + /// \return True if both outputs were set low; false otherwise. + bool stop(); + + /// Get the last commanded normalized duty cycles. + /// \return Array containing output A then output B duty cycles in the range + /// [0.0, 1.0]. + std::array duty_cycle() const; + + /// Get the last commanded raw duty counts. + /// \return Array containing output A then output B raw compare values. + std::array raw_duty() const; + + /// Get the maximum raw duty count for this driver's timer configuration. + /// \return Maximum raw duty count corresponding to 100% duty. + uint32_t max_raw_duty() const; + + /// Get the configured PWM carrier frequency. + /// \return PWM carrier frequency in Hz. + size_t pwm_frequency_hz() const; + + /// Get the configured timer resolution. + /// \return MCPWM timer resolution in Hz. + size_t timer_resolution_hz() const; + + /// Get the configured MCPWM group. + /// \return MCPWM group index used by this driver. + int group_id() const; + + /// Get output A's GPIO. + /// \return Output A GPIO. + gpio_num_t gpio_a() const; + + /// Get output B's GPIO. + /// \return Output B GPIO. + gpio_num_t gpio_b() const; + +protected: + bool init(const Config &config); + bool configure_enable_gpio(); + bool configure_timer_and_operator(); + bool configure_outputs(const Config &config); + bool configure_generator(mcpwm_gen_handle_t generator, mcpwm_cmpr_handle_t comparator); + bool apply_output(size_t index, float duty); + void cleanup(); + + gpio_num_t gpio_a_{GPIO_NUM_NC}; + gpio_num_t gpio_b_{GPIO_NUM_NC}; + gpio_num_t gpio_enable_{GPIO_NUM_NC}; + int group_id_{0}; + size_t timer_resolution_hz_{DEFAULT_TIMER_RESOLUTION_HZ}; + size_t pwm_frequency_hz_{DEFAULT_PWM_FREQUENCY_HZ}; + uint32_t period_ticks_{0}; + bool initialized_{false}; + std::atomic enabled_{false}; + mcpwm_timer_handle_t timer_{nullptr}; + mcpwm_oper_handle_t operator_{nullptr}; + std::array comparators_{}; + std::array generators_{}; + std::array duty_cycle_{0.0f, 0.0f}; + std::array raw_duty_{0, 0}; +}; // class BdcDriver +} // namespace espp diff --git a/components/bdc_driver/src/bdc_driver.cpp b/components/bdc_driver/src/bdc_driver.cpp new file mode 100644 index 000000000..0ae9706ac --- /dev/null +++ b/components/bdc_driver/src/bdc_driver.cpp @@ -0,0 +1,342 @@ +#include "bdc_driver.hpp" + +#include + +using namespace espp; + +BdcDriver::BdcDriver(const Config &config) + : BaseComponent("BdcDriver", config.log_level) + , gpio_a_(config.gpio_a) + , gpio_b_(config.gpio_b) + , gpio_enable_(config.gpio_enable) + , group_id_(config.group_id) + , timer_resolution_hz_(config.timer_resolution_hz) + , pwm_frequency_hz_(config.pwm_frequency_hz) { + initialized_ = init(config); + if (!initialized_) { + cleanup(); + } +} + +BdcDriver::~BdcDriver() { cleanup(); } + +bool BdcDriver::initialized() const { return initialized_; } + +bool BdcDriver::enable() { + if (!initialized_) { + logger_.error("Cannot enable brushed motor driver before initialization"); + return false; + } + if (enabled_) { + return true; + } + esp_err_t err = mcpwm_timer_enable(timer_); + if (err != ESP_OK) { + logger_.error("Failed to enable MCPWM timer: {}", esp_err_to_name(err)); + return false; + } + err = mcpwm_timer_start_stop(timer_, MCPWM_TIMER_START_NO_STOP); + if (err != ESP_OK) { + logger_.error("Failed to start MCPWM timer: {}", esp_err_to_name(err)); + mcpwm_timer_disable(timer_); + return false; + } + if (gpio_enable_ != GPIO_NUM_NC) { + gpio_set_level(gpio_enable_, 1); + } + enabled_ = true; + return true; +} + +bool BdcDriver::disable() { + if (!timer_) { + return true; + } + bool ok = true; + if (gpio_enable_ != GPIO_NUM_NC) { + gpio_set_level(gpio_enable_, 0); + } + for (auto &generator : generators_) { + if (!generator) { + continue; + } + esp_err_t err = mcpwm_generator_set_force_level(generator, 0, true); + if (err != ESP_OK) { + logger_.error("Failed to force MCPWM output low during disable: {}", esp_err_to_name(err)); + ok = false; + } + } + if (enabled_) { + esp_err_t err = mcpwm_timer_start_stop(timer_, MCPWM_TIMER_STOP_FULL); + if (err != ESP_OK) { + logger_.error("Failed to stop MCPWM timer: {}", esp_err_to_name(err)); + ok = false; + } + err = mcpwm_timer_disable(timer_); + if (err != ESP_OK) { + logger_.error("Failed to disable MCPWM timer: {}", esp_err_to_name(err)); + ok = false; + } + enabled_ = false; + } + duty_cycle_ = {0.0f, 0.0f}; + raw_duty_ = {0, 0}; + return ok; +} + +bool BdcDriver::is_enabled() const { return enabled_; } + +bool BdcDriver::set_duty(float duty_a, float duty_b) { + if (!initialized_) { + logger_.error("Cannot set brushed motor duty before initialization"); + return false; + } + if (!enabled_) { + logger_.error("Cannot set brushed motor duty while disabled"); + return false; + } + bool ok_a = apply_output(0, duty_a); + bool ok_b = apply_output(1, duty_b); + return ok_a && ok_b; +} + +bool BdcDriver::set_speed(float speed) { + speed = std::clamp(speed, -1.0f, 1.0f); + if (speed > 0.0f) { + return set_duty(speed, 0.0f); + } + if (speed < 0.0f) { + return set_duty(0.0f, -speed); + } + return stop(); +} + +bool BdcDriver::stop() { + if (!initialized_) { + logger_.error("Cannot stop brushed motor driver before initialization"); + return false; + } + bool ok_a = apply_output(0, 0.0f); + bool ok_b = apply_output(1, 0.0f); + return ok_a && ok_b; +} + +std::array BdcDriver::duty_cycle() const { return duty_cycle_; } + +std::array BdcDriver::raw_duty() const { return raw_duty_; } + +uint32_t BdcDriver::max_raw_duty() const { return period_ticks_; } + +size_t BdcDriver::pwm_frequency_hz() const { return pwm_frequency_hz_; } + +size_t BdcDriver::timer_resolution_hz() const { return timer_resolution_hz_; } + +int BdcDriver::group_id() const { return group_id_; } + +gpio_num_t BdcDriver::gpio_a() const { return gpio_a_; } + +gpio_num_t BdcDriver::gpio_b() const { return gpio_b_; } + +bool BdcDriver::init(const Config &config) { + if (gpio_a_ == GPIO_NUM_NC || gpio_b_ == GPIO_NUM_NC) { + logger_.error("BDC driver requires two valid PWM GPIOs"); + return false; + } + if (group_id_ < 0) { + logger_.error("BDC driver requires a non-negative MCPWM group id"); + return false; + } + if (timer_resolution_hz_ == 0 || pwm_frequency_hz_ == 0) { + logger_.error("BDC driver requires non-zero timer resolution and PWM frequency"); + return false; + } + period_ticks_ = timer_resolution_hz_ / pwm_frequency_hz_; + if (period_ticks_ < 2) { + logger_.error("BDC driver timer period is too small: {} ticks", period_ticks_); + return false; + } + if (!configure_enable_gpio()) { + return false; + } + if (!configure_timer_and_operator()) { + return false; + } + if (!configure_outputs(config)) { + return false; + } + if (!apply_output(0, 0.0f) || !apply_output(1, 0.0f)) { + return false; + } + initialized_ = true; + if (!enable()) { + initialized_ = false; + return false; + } + return true; +} + +bool BdcDriver::configure_enable_gpio() { + if (gpio_enable_ == GPIO_NUM_NC) { + return true; + } + gpio_config_t config{}; + config.pin_bit_mask = 1ULL << gpio_enable_; + config.mode = GPIO_MODE_OUTPUT; + esp_err_t err = gpio_config(&config); + if (err != ESP_OK) { + logger_.error("Failed to configure BDC enable GPIO {}: {}", static_cast(gpio_enable_), + esp_err_to_name(err)); + return false; + } + gpio_set_level(gpio_enable_, 0); + return true; +} + +bool BdcDriver::configure_timer_and_operator() { + mcpwm_timer_config_t timer_config{}; + timer_config.group_id = group_id_; + timer_config.clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT; + timer_config.resolution_hz = timer_resolution_hz_; + timer_config.count_mode = MCPWM_TIMER_COUNT_MODE_UP; + timer_config.period_ticks = period_ticks_; + esp_err_t err = mcpwm_new_timer(&timer_config, &timer_); + if (err != ESP_OK) { + logger_.error("Failed to create MCPWM timer in group {}: {}", group_id_, esp_err_to_name(err)); + return false; + } + + mcpwm_operator_config_t operator_config{}; + operator_config.group_id = group_id_; + err = mcpwm_new_operator(&operator_config, &operator_); + if (err != ESP_OK) { + logger_.error("Failed to create MCPWM operator in group {}: {}", group_id_, + esp_err_to_name(err)); + return false; + } + + err = mcpwm_operator_connect_timer(operator_, timer_); + if (err != ESP_OK) { + logger_.error("Failed to connect MCPWM operator to timer: {}", esp_err_to_name(err)); + return false; + } + return true; +} + +bool BdcDriver::configure_outputs(const Config &config) { + mcpwm_comparator_config_t comparator_config{}; + comparator_config.flags.update_cmp_on_tez = true; + mcpwm_generator_config_t generator_a_config{}; + generator_a_config.gen_gpio_num = gpio_a_; + generator_a_config.flags.invert_pwm = config.invert_output_a; + mcpwm_generator_config_t generator_b_config{}; + generator_b_config.gen_gpio_num = gpio_b_; + generator_b_config.flags.invert_pwm = config.invert_output_b; + + const std::array generator_configs{generator_a_config, + generator_b_config}; + for (size_t i = 0; i < comparators_.size(); i++) { + esp_err_t err = mcpwm_new_comparator(operator_, &comparator_config, &comparators_[i]); + if (err != ESP_OK) { + logger_.error("Failed to create MCPWM comparator {}: {}", i, esp_err_to_name(err)); + return false; + } + err = mcpwm_comparator_set_compare_value(comparators_[i], 0); + if (err != ESP_OK) { + logger_.error("Failed to initialize MCPWM comparator {}: {}", i, esp_err_to_name(err)); + return false; + } + err = mcpwm_new_generator(operator_, &generator_configs[i], &generators_[i]); + if (err != ESP_OK) { + logger_.error("Failed to create MCPWM generator {}: {}", i, esp_err_to_name(err)); + return false; + } + if (!configure_generator(generators_[i], comparators_[i])) { + return false; + } + } + return true; +} + +bool BdcDriver::configure_generator(mcpwm_gen_handle_t generator, mcpwm_cmpr_handle_t comparator) { + esp_err_t err = mcpwm_generator_set_action_on_timer_event( + generator, MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, + MCPWM_GEN_ACTION_HIGH)); + if (err != ESP_OK) { + logger_.error("Failed to set MCPWM generator empty action: {}", esp_err_to_name(err)); + return false; + } + err = mcpwm_generator_set_action_on_timer_event( + generator, MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_FULL, + MCPWM_GEN_ACTION_LOW)); + if (err != ESP_OK) { + logger_.error("Failed to set MCPWM generator full action: {}", esp_err_to_name(err)); + return false; + } + err = mcpwm_generator_set_action_on_compare_event( + generator, + MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, comparator, MCPWM_GEN_ACTION_LOW)); + if (err != ESP_OK) { + logger_.error("Failed to set MCPWM generator compare action: {}", esp_err_to_name(err)); + return false; + } + return true; +} + +bool BdcDriver::apply_output(size_t index, float duty) { + duty = std::clamp(duty, 0.0f, 1.0f); + mcpwm_gen_handle_t generator = generators_[index]; + mcpwm_cmpr_handle_t comparator = comparators_[index]; + if (!generator || !comparator) { + logger_.error("MCPWM output {} is not initialized", index); + return false; + } + + esp_err_t err = ESP_OK; + uint32_t raw = 0; + if (duty <= 0.0f) { + err = mcpwm_generator_set_force_level(generator, 0, true); + } else if (duty >= 1.0f) { + raw = period_ticks_; + err = mcpwm_generator_set_force_level(generator, 1, true); + } else { + const uint32_t max_compare = period_ticks_ - static_cast(1); + raw = std::clamp(static_cast(duty * static_cast(period_ticks_) + 0.5f), + static_cast(1), max_compare); + err = mcpwm_comparator_set_compare_value(comparator, raw); + if (err == ESP_OK) { + err = mcpwm_generator_set_force_level(generator, -1, true); + } + } + if (err != ESP_OK) { + logger_.error("Failed to update MCPWM output {}: {}", index, esp_err_to_name(err)); + return false; + } + duty_cycle_[index] = duty; + raw_duty_[index] = raw; + return true; +} + +void BdcDriver::cleanup() { + disable(); + for (auto &generator : generators_) { + if (generator) { + mcpwm_del_generator(generator); + generator = nullptr; + } + } + for (auto &comparator : comparators_) { + if (comparator) { + mcpwm_del_comparator(comparator); + comparator = nullptr; + } + } + if (operator_) { + mcpwm_del_operator(operator_); + operator_ = nullptr; + } + if (timer_) { + mcpwm_del_timer(timer_); + timer_ = nullptr; + } + initialized_ = false; +} diff --git a/components/motorgo-plink/CMakeLists.txt b/components/motorgo-plink/CMakeLists.txt index cfb3e2a8b..95556a42c 100644 --- a/components/motorgo-plink/CMakeLists.txt +++ b/components/motorgo-plink/CMakeLists.txt @@ -1,6 +1,6 @@ idf_component_register( INCLUDE_DIRS "include" SRC_DIRS "src" - REQUIRES base_component esp_driver_gpio i2c led lsm6dso math mt6701 spi task + REQUIRES base_component bdc_driver esp_driver_gpio i2c led lsm6dso math mt6701 spi task REQUIRED_IDF_TARGETS "esp32s3" ) diff --git a/components/motorgo-plink/README.md b/components/motorgo-plink/README.md index 18b478a15..b50235fd3 100644 --- a/components/motorgo-plink/README.md +++ b/components/motorgo-plink/README.md @@ -16,8 +16,8 @@ The `espp::MotorGoPlink` component provides a singleton board abstraction for those documented pin mappings, plus convenient helpers for: - initializing and driving the four DC-motor channels with normalized speed - commands, where positive commands drive `pwm_a`, negative commands drive - `pwm_b`, and zero disables both outputs + commands backed by `espp::BdcDriver` on MCPWM, where positive commands drive + `pwm_a`, negative commands drive `pwm_b`, and zero disables both outputs - initializing the shared encoder SPI/SSI bus and creating four `espp::Mt6701` encoder instances - initializing the onboard LSM6DS33 IMU on the hidden I2C bus via the shared @@ -33,5 +33,5 @@ those documented pin mappings, plus convenient helpers for: The [example](./example) initializes the board, logs the MotorGo Plink pin map, starts the onboard LEDs breathing, initializes the onboard IMU, and safely keeps the motors stopped by default. Optional `menuconfig` flags let you enable -encoder polling and a slow motor sweep when you are connected to real hardware -and want to exercise the board interfaces. +encoder polling and a stronger motor sweep while the LEDs continue pulsing, +allowing you to exercise the board interfaces together on real hardware. diff --git a/components/motorgo-plink/example/README.md b/components/motorgo-plink/example/README.md index ac1bd4a0e..6d8d1c0dc 100644 --- a/components/motorgo-plink/example/README.md +++ b/components/motorgo-plink/example/README.md @@ -6,13 +6,18 @@ initialize the hardware on the MotorGo Plink board. By default the example is intentionally safe: - it logs the documented motor, encoder, servo, I2C, and LED pin mappings -- it starts the user and status LEDs breathing -- it initializes the motor PWM outputs and keeps all four motors stopped +- it starts the user and status LEDs breathing out of phase +- it leaves the motor PWM outputs disabled unless the motor sweep demo is enabled Optional `menuconfig` flags let you also: - initialize and poll the four encoder inputs -- run a slow sinusoidal motor sweep across the four channels +- run a stronger sinusoidal motor sweep across the four channels, with a zero + crossing window and an active command range chosen to push past typical motor + deadband + +The indicator LEDs continue pulsing while the motor sweep is active because the +motor outputs use MCPWM instead of LEDC. ## How to use example diff --git a/components/motorgo-plink/example/main/motorgo_plink_example.cpp b/components/motorgo-plink/example/main/motorgo_plink_example.cpp index 66a003c21..be63101c2 100644 --- a/components/motorgo-plink/example/main/motorgo_plink_example.cpp +++ b/components/motorgo-plink/example/main/motorgo_plink_example.cpp @@ -88,17 +88,46 @@ void log_encoder_angles(espp::MotorGoPlink &board) { #if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP void update_motor_demo(espp::MotorGoPlink &board, float &phase) { - constexpr float amplitude = 0.20f; - constexpr float phase_step = 0.20f; + constexpr float min_active_command = 0.35f; + constexpr float max_command = 0.85f; + constexpr float zero_window = 0.05f; + constexpr float phase_step = 0.15f; constexpr float channel_offset = std::numbers::pi_v / 2.0f; for (size_t i = 0; i < espp::MotorGoPlink::num_motor_channels(); i++) { - float speed = amplitude * std::sin(phase + channel_offset * i); + float waveform = std::sin(phase + channel_offset * i); + float speed = 0.0f; + float magnitude = std::abs(waveform); + if (magnitude > zero_window) { + float normalized = (magnitude - zero_window) / (1.0f - zero_window); + float commanded_magnitude = + min_active_command + (max_command - min_active_command) * normalized; + speed = std::copysign(commanded_magnitude, waveform); + } board.set_motor_speed(i, speed); } phase = std::fmod(phase + phase_step, 2.0f * std::numbers::pi_v); } + +void log_motor_outputs(espp::MotorGoPlink &board) { + for (size_t i = 0; i < espp::MotorGoPlink::num_motor_channels(); i++) { + auto driver = board.motor_driver(i); + auto pins = board.motor_pins(i); + if (!driver) { + logger.warn("Motor {} driver is not initialized", i + 1); + continue; + } + auto duty = driver->duty_cycle(); + auto raw = driver->raw_duty(); + auto max_raw_duty = driver->max_raw_duty(); + + logger.info("Motor {} cmd={:.3f} | pwm_a gpio={} duty={:.1f}% raw={}/{} | pwm_b gpio={} " + "duty={:.1f}% raw={}/{}", + i + 1, board.motor_speed(i), static_cast(pins.pwm_a), duty[0] * 100.0f, raw[0], + max_raw_duty, static_cast(pins.pwm_b), duty[1] * 100.0f, raw[1], max_raw_duty); + } +} #endif } // namespace @@ -111,11 +140,15 @@ extern "C" void app_main() { board.start_led_breathing(); } +#if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP if (!board.initialize_motors()) { - logger.error("Failed to initialize motor PWM outputs"); + logger.error("Failed to initialize motor drivers"); } else { board.stop_all_motors(); } +#else + logger.info("Motor drivers not initialized because motor sweep is disabled"); +#endif if (!board.initialize_imu()) { logger.warn("Failed to initialize onboard IMU"); @@ -136,7 +169,9 @@ extern "C" void app_main() { #endif #if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP - logger.warn("Motor sweep enabled; ensure the board is connected to a safe test setup"); + logger.warn("Motor sweep enabled; using an aggressive {:.0f}% to {:.0f}% duty sweep after a " + "{:.0f}% zero window. Ensure the board is connected to a safe test setup", + 35.0f, 85.0f, 5.0f); #else logger.info("Motor sweep disabled; all motors remain stopped"); #endif @@ -147,6 +182,7 @@ extern "C" void app_main() { while (true) { #if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_MOTOR_SWEEP update_motor_demo(board, motor_phase); + log_motor_outputs(board); #endif #if CONFIG_MOTORGO_PLINK_EXAMPLE_ENABLE_ENCODER_POLLING diff --git a/components/motorgo-plink/idf_component.yml b/components/motorgo-plink/idf_component.yml index 9760239e3..61512d04d 100644 --- a/components/motorgo-plink/idf_component.yml +++ b/components/motorgo-plink/idf_component.yml @@ -22,6 +22,7 @@ dependencies: idf: version: '>=5.0' espp/base_component: '>=1.0' + espp/bdc_driver: '>=1.0' espp/i2c: '>=1.0' espp/led: '>=1.0' espp/lsm6dso: '>=1.0' diff --git a/components/motorgo-plink/include/motorgo-plink.hpp b/components/motorgo-plink/include/motorgo-plink.hpp index 7338d236d..a94f0b1fa 100644 --- a/components/motorgo-plink/include/motorgo-plink.hpp +++ b/components/motorgo-plink/include/motorgo-plink.hpp @@ -10,6 +10,7 @@ #include #include "base_component.hpp" +#include "bdc_driver.hpp" #include "gaussian.hpp" #include "i2c.hpp" #include "led.hpp" @@ -39,6 +40,8 @@ class MotorGoPlink : public BaseComponent { /// Alias for the SSI-based magnetic encoder helper used on each motor /// channel. using Encoder = espp::Mt6701; + /// Alias for the brushed DC motor driver helper used on each motor channel. + using MotorDriver = espp::BdcDriver; /// Alias for the onboard hidden-bus IMU helper. using Imu = espp::Lsm6dso; @@ -144,11 +147,10 @@ class MotorGoPlink : public BaseComponent { /// \return The documented user and status LED GPIOs. LedPins led_pins() const; - /// Initialize the eight PWM outputs used by the four motor channels. + /// Initialize the four MCPWM-backed motor driver helpers. /// \param pwm_frequency_hz PWM carrier frequency for all four motor channels. - /// \return True if the motor PWM helper was initialized; - /// false if the PWM frequency is invalid or the motor outputs were - /// already initialized. + /// \return True if the motor driver helpers were initialized; false if the PWM + /// frequency is invalid or any motor channel could not be configured. bool initialize_motors(size_t pwm_frequency_hz = motor_default_pwm_frequency_hz()); /// Set a normalized motor speed in the range [-1, 1]. @@ -176,10 +178,11 @@ class MotorGoPlink : public BaseComponent { /// values are simply reset to zero. void stop_all_motors(); - /// Get the LEDC wrapper used for the eight motor PWM outputs. - /// \return Shared pointer to the motor PWM helper, or `nullptr` if - /// initialize_motors() has not been called yet. - std::shared_ptr motor_pwm(); + /// Get one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return Shared pointer to the requested motor driver helper, or `nullptr` + /// if the index is invalid or that motor has not been initialized yet. + std::shared_ptr motor_driver(size_t index) const; /// Initialize the shared encoder bus and create the four MT6701 SSI helpers. /// \param run_tasks If true, each encoder starts its own update task after @@ -227,7 +230,8 @@ class MotorGoPlink : public BaseComponent { /// \param breathing_period Default breathing period in seconds for /// start_led_breathing(). /// \return True if the indicator LED PWM helper and breathing task were - /// created successfully; false if they were already initialized. + /// created successfully; false if they were already initialized or the + /// LEDs could not be driven after setup. bool initialize_leds(float breathing_period = 3.5f); /// Start breathing both indicator LEDs. @@ -308,7 +312,7 @@ class MotorGoPlink : public BaseComponent { bool initialize_encoder_spi(); bool read_encoder(size_t index, uint8_t *data, size_t size); - float led_breathe(); + float led_breathe(float phase_offset = 0.0f); bool led_task_callback(std::mutex &m, std::condition_variable &cv, bool &task_notified); static constexpr std::array motor_pin_map_{{ @@ -349,6 +353,7 @@ class MotorGoPlink : public BaseComponent { static constexpr size_t motor_default_pwm_frequency_hz_ = 20 * 1000; static constexpr size_t indicator_led_pwm_frequency_hz_ = 5 * 1000; static constexpr float encoder_update_period_seconds_ = 0.001f; + static constexpr std::array motor_driver_group_ids_{0, 0, 0, 1}; I2c qwiic_i2c_{{.port = I2C_NUM_0, .sda_io_num = qwiic_pin_map_.sda, @@ -367,22 +372,12 @@ class MotorGoPlink : public BaseComponent { std::shared_ptr> imu_i2c_device_; std::shared_ptr imu_; - std::array motor_channels_{{ - {static_cast(motor_pin_map_[0].pwm_a), LEDC_CHANNEL_0, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[0].pwm_b), LEDC_CHANNEL_1, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[1].pwm_a), LEDC_CHANNEL_2, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[1].pwm_b), LEDC_CHANNEL_3, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[2].pwm_a), LEDC_CHANNEL_4, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[2].pwm_b), LEDC_CHANNEL_5, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[3].pwm_a), LEDC_CHANNEL_6, LEDC_TIMER_0}, - {static_cast(motor_pin_map_[3].pwm_b), LEDC_CHANNEL_7, LEDC_TIMER_0}, - }}; std::array led_channels_{{ {static_cast(user_led_pin_), LEDC_CHANNEL_4, LEDC_TIMER_1}, {static_cast(status_led_pin_), LEDC_CHANNEL_5, LEDC_TIMER_1}, }}; - std::shared_ptr motor_pwm_; + std::array, 4> motor_drivers_{}; std::shared_ptr indicator_leds_; std::array motor_speeds_{0.0f, 0.0f, 0.0f, 0.0f}; std::unique_ptr led_task_; diff --git a/components/motorgo-plink/src/motorgo-plink.cpp b/components/motorgo-plink/src/motorgo-plink.cpp index c01bddb51..81edc1337 100644 --- a/components/motorgo-plink/src/motorgo-plink.cpp +++ b/components/motorgo-plink/src/motorgo-plink.cpp @@ -55,53 +55,48 @@ MotorGoPlink::LedPins MotorGoPlink::led_pins() const { } bool MotorGoPlink::initialize_motors(size_t pwm_frequency_hz) { - if (motor_pwm_) { - logger_.error("Motor PWM already initialized"); + if (std::any_of(motor_drivers_.begin(), motor_drivers_.end(), + [](const auto &driver) { return static_cast(driver); })) { + logger_.error("Motor drivers already initialized"); return false; } if (pwm_frequency_hz == 0) { logger_.error("Motor PWM frequency must be greater than zero"); return false; } - motor_pwm_ = std::make_shared(espp::Led::Config{ - .timer = LEDC_TIMER_0, - .frequency_hz = pwm_frequency_hz, - .channels = - std::vector(motor_channels_.begin(), motor_channels_.end()), - .duty_resolution = LEDC_TIMER_13_BIT, - .log_level = get_log_level(), - }); + for (size_t i = 0; i < motor_drivers_.size(); i++) { + auto pins = motor_pin_map_[i]; + auto driver = std::make_shared(MotorDriver::Config{ + .gpio_a = pins.pwm_a, + .gpio_b = pins.pwm_b, + .group_id = motor_driver_group_ids_[i], + .pwm_frequency_hz = pwm_frequency_hz, + .log_level = get_log_level(), + }); + if (!driver || !driver->initialized()) { + logger_.error("Failed to initialize motor driver {}", i + 1); + stop_all_motors(); + motor_drivers_ = {}; + motor_speeds_.fill(0.0f); + return false; + } + motor_drivers_[i] = driver; + } stop_all_motors(); return true; } bool MotorGoPlink::set_motor_speed(size_t index, float speed) { - if (!motor_pwm_) { - logger_.error("Motor PWM not initialized"); - return false; - } if (index >= motor_pin_map_.size()) { logger_.error("Invalid motor index: {}", index); return false; } + if (!motor_drivers_[index]) { + logger_.error("Motor driver {} not initialized", index + 1); + return false; + } speed = std::clamp(speed, -1.0f, 1.0f); - float duty = 100.0f * std::abs(speed); - size_t pwm_a_index = index * 2; - size_t pwm_b_index = pwm_a_index + 1; - - bool ok_a = true; - bool ok_b = true; - if (speed > 0.0f) { - ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, 0.0f); - ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, duty); - } else if (speed < 0.0f) { - ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, 0.0f); - ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, duty); - } else { - ok_a = motor_pwm_->set_duty(motor_channels_[pwm_a_index].channel, 0.0f); - ok_b = motor_pwm_->set_duty(motor_channels_[pwm_b_index].channel, 0.0f); - } - bool ok = ok_a && ok_b; + bool ok = motor_drivers_[index]->set_speed(speed); if (ok) { motor_speeds_[index] = speed; } @@ -119,16 +114,21 @@ float MotorGoPlink::motor_speed(size_t index) const { void MotorGoPlink::stop_motor(size_t index) { set_motor_speed(index, 0.0f); } void MotorGoPlink::stop_all_motors() { - if (!motor_pwm_) { - motor_speeds_.fill(0.0f); - return; - } - for (size_t i = 0; i < motor_pin_map_.size(); i++) { - set_motor_speed(i, 0.0f); + for (size_t i = 0; i < motor_drivers_.size(); i++) { + if (motor_drivers_[i]) { + motor_drivers_[i]->stop(); + } + motor_speeds_[i] = 0.0f; } } -std::shared_ptr MotorGoPlink::motor_pwm() { return motor_pwm_; } +std::shared_ptr MotorGoPlink::motor_driver(size_t index) const { + if (index >= motor_drivers_.size()) { + logger_.error("Invalid motor index: {}", index); + return nullptr; + } + return motor_drivers_[index]; +} bool MotorGoPlink::initialize_encoders(bool run_tasks) { if (!initialize_encoder_spi()) { @@ -203,12 +203,18 @@ bool MotorGoPlink::initialize_imu(const Imu::filter_fn &orientation_filter, .read = espp::make_i2c_addressed_read(imu_i2c_device_), .imu_config = imu_config, .orientation_filter = orientation_filter, - .auto_init = true, + .auto_init = false, .log_level = get_log_level(), }; logger_.info("Initializing hidden-bus IMU"); imu_ = std::make_shared(config); + if (!imu_ || !imu_->init(ec)) { + logger_.error("Failed to initialize hidden-bus IMU: {}", ec.message()); + imu_.reset(); + imu_i2c_device_.reset(); + return false; + } return true; } @@ -230,9 +236,23 @@ bool MotorGoPlink::initialize_leds(float breathing_period) { led_task_ = espp::Task::make_unique( {.callback = std::bind(&MotorGoPlink::led_task_callback, this, _1, _2, _3), .task_config = {.name = "motorgo_plink_led"}}); - set_led_breathing_period(breathing_period); - set_user_led_brightness(0.0f); - set_status_led_brightness(0.0f); + if (!led_task_) { + logger_.error("Failed to create indicator LED task"); + indicator_leds_.reset(); + return false; + } + if (!set_led_breathing_period(breathing_period)) { + led_task_.reset(); + indicator_leds_.reset(); + return false; + } + if (!indicator_leds_->set_duty(led_channels_[0].channel, 0.0f) || + !indicator_leds_->set_duty(led_channels_[1].channel, 0.0f)) { + logger_.error("Failed to initialize indicator LED channels"); + led_task_.reset(); + indicator_leds_.reset(); + return false; + } return true; } @@ -361,19 +381,21 @@ bool IRAM_ATTR MotorGoPlink::read_encoder(size_t index, uint8_t *data, size_t si return encoder_spi_devices_[index]->read(std::span(data, size), {}, ec); } -float MotorGoPlink::led_breathe() { +float MotorGoPlink::led_breathe(float phase_offset) { auto now = std::chrono::high_resolution_clock::now(); auto elapsed = std::chrono::duration(now - breathing_start_).count(); float t = std::fmod(elapsed, breathing_period_) / breathing_period_; + t = std::fmod(t + phase_offset, 1.0f); return gaussian_(t); } bool MotorGoPlink::led_task_callback(std::mutex &m, std::condition_variable &cv, bool &task_notified) { using namespace std::chrono_literals; - float brightness = led_breathe(); - indicator_leds_->set_duty(led_channels_[0].channel, 100.0f * brightness); - indicator_leds_->set_duty(led_channels_[1].channel, 100.0f * brightness); + float user_brightness = led_breathe(0.0f); + float status_brightness = led_breathe(0.5f); + indicator_leds_->set_duty(led_channels_[0].channel, 100.0f * user_brightness); + indicator_leds_->set_duty(led_channels_[1].channel, 100.0f * status_brightness); std::unique_lock lk(m); cv.wait_for(lk, 10ms, [&task_notified] { return task_notified; }); task_notified = false; diff --git a/doc/Doxyfile b/doc/Doxyfile index 03b2b0ddc..bc336c5eb 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -75,6 +75,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/adxl345/example/main/adxl345_example.cpp \ $(PROJECT_PATH)/components/as5600/example/main/as5600_example.cpp \ $(PROJECT_PATH)/components/aw9523/example/main/aw9523_example.cpp \ + $(PROJECT_PATH)/components/bdc_driver/example/main/bdc_driver_example.cpp \ $(PROJECT_PATH)/components/lp5817/example/main/lp5817_example.cpp \ $(PROJECT_PATH)/components/binary-log/example/main/binary_log_example.cpp \ $(PROJECT_PATH)/components/ble_gatt_server/example/main/ble_gatt_server_example.cpp \ @@ -126,6 +127,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/max1704x/example/main/max1704x_example.cpp \ $(PROJECT_PATH)/components/monitor/example/main/monitor_example.cpp \ $(PROJECT_PATH)/components/motorgo-mini/example/main/motorgo_mini_example.cpp \ + $(PROJECT_PATH)/components/motorgo-plink/example/main/motorgo_plink_example.cpp \ $(PROJECT_PATH)/components/mcp23x17/example/main/mcp23x17_example.cpp \ $(PROJECT_PATH)/components/mt6701/example/main/mt6701_example.cpp \ $(PROJECT_PATH)/components/neopixel/example/main/neopixel_example.cpp \ @@ -185,6 +187,7 @@ INPUT = \ $(PROJECT_PATH)/components/aw9523/include/aw9523.hpp \ $(PROJECT_PATH)/components/base_component/include/base_component.hpp \ $(PROJECT_PATH)/components/base_peripheral/include/base_peripheral.hpp \ + $(PROJECT_PATH)/components/bdc_driver/include/bdc_driver.hpp \ $(PROJECT_PATH)/components/binary-log/include/binary-log.hpp \ $(PROJECT_PATH)/components/ble_gatt_server/include/battery_service.hpp \ $(PROJECT_PATH)/components/ble_gatt_server/include/ble_appearances.hpp \ @@ -298,6 +301,7 @@ INPUT = \ $(PROJECT_PATH)/components/monitor/include/heap_monitor.hpp \ $(PROJECT_PATH)/components/monitor/include/task_monitor.hpp \ $(PROJECT_PATH)/components/motorgo-mini/include/motorgo-mini.hpp \ + $(PROJECT_PATH)/components/motorgo-plink/include/motorgo-plink.hpp \ $(PROJECT_PATH)/components/ndef/include/ndef.hpp \ $(PROJECT_PATH)/components/neopixel/include/neopixel.hpp \ $(PROJECT_PATH)/components/nvs/include/nvs.hpp \ diff --git a/doc/en/bldc/bdc_driver.rst b/doc/en/bldc/bdc_driver.rst new file mode 100644 index 000000000..fe08619ac --- /dev/null +++ b/doc/en/bldc/bdc_driver.rst @@ -0,0 +1,23 @@ +BDC Driver +********** + +The `BdcDriver` component wraps the `ESP MCPWM Peripheral +`_ +to provide dual-PWM control for brushed DC motors and H-bridge style drivers. + +It supports direct duty-cycle control of the two motor outputs as well as a +signed speed helper that maps positive commands to output A and negative +commands to output B. + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + bdc_driver_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/bdc_driver.inc diff --git a/doc/en/bldc/bdc_driver_example.md b/doc/en/bldc/bdc_driver_example.md new file mode 100644 index 000000000..dea771f85 --- /dev/null +++ b/doc/en/bldc/bdc_driver_example.md @@ -0,0 +1,2 @@ +```{include} ../../../components/bdc_driver/example/README.md +``` diff --git a/doc/en/bldc/index.rst b/doc/en/bldc/index.rst index 852734e43..ba8cf458d 100644 --- a/doc/en/bldc/index.rst +++ b/doc/en/bldc/index.rst @@ -1,15 +1,17 @@ -BLDC APIs -********* +Motor (BDC + BLDC) APIs +*********************** .. toctree:: :maxdepth: 1 + bdc_driver bldc_driver bldc_motor -These components provide interfaces by which the user can control brushless DC -(BLDC) motors. The driver component(s) implement the low-level voltage / pwm -output to the motor directly, where the motor component(s) implement the -open-loop or closed-loop control algorithms - using the driver. +These components provide interfaces by which the user can control brushed DC (BDC) and +brushless DC (BLDC) motors. The driver component(s) implement the low-level +voltage / pwm output to the motor directly, where the motor component(s) +implement the open-loop or closed-loop control algorithms using the driver. -Code examples for the BLDC API are provided in the `bldc_motor` example folder. +Code examples for the motor-driver APIs are provided in the `bdc_driver` and +`bldc_motor` example folders. diff --git a/doc/en/index.rst b/doc/en/index.rst index 6922cddbf..ab3c34c57 100755 --- a/doc/en/index.rst +++ b/doc/en/index.rst @@ -51,6 +51,7 @@ This is the documentation for esp-idf c++ components, ESPP (`espp ` helpers - initializing the onboard LSM6DS33 IMU on the hidden I2C bus via the From ca9a688620b90c394ca3af314d539aa43427b27d Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 8 Jun 2026 15:58:49 -0500 Subject: [PATCH 3/4] fix sa --- components/motorgo-plink/src/motorgo-plink.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/components/motorgo-plink/src/motorgo-plink.cpp b/components/motorgo-plink/src/motorgo-plink.cpp index 81edc1337..a375c9c52 100644 --- a/components/motorgo-plink/src/motorgo-plink.cpp +++ b/components/motorgo-plink/src/motorgo-plink.cpp @@ -73,7 +73,7 @@ bool MotorGoPlink::initialize_motors(size_t pwm_frequency_hz) { .pwm_frequency_hz = pwm_frequency_hz, .log_level = get_log_level(), }); - if (!driver || !driver->initialized()) { + if (!driver->initialized()) { logger_.error("Failed to initialize motor driver {}", i + 1); stop_all_motors(); motor_drivers_ = {}; From 2c31fe63c56e3f4de3c6c286d66944ac9ba313ef Mon Sep 17 00:00:00 2001 From: William Emfinger Date: Mon, 8 Jun 2026 16:22:39 -0500 Subject: [PATCH 4/4] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- components/motorgo-plink/src/motorgo-plink.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/components/motorgo-plink/src/motorgo-plink.cpp b/components/motorgo-plink/src/motorgo-plink.cpp index a375c9c52..cb34ceefb 100644 --- a/components/motorgo-plink/src/motorgo-plink.cpp +++ b/components/motorgo-plink/src/motorgo-plink.cpp @@ -367,6 +367,8 @@ bool MotorGoPlink::initialize_encoder_spi() { ec); if (ec || !encoder_spi_devices_[i]) { logger_.error("Failed to initialize encoder {} SPI device: {}", i + 1, ec.message()); + encoder_spi_devices_ = {}; + encoder_spi_.reset(); return false; } }