diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index ed5cdb315..04c6cce78 100755 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -141,6 +141,8 @@ jobs: target: esp32 - path: 'components/monitor/example' target: esp32 + - path: 'components/motorgo-axis/example' + target: esp32s3 - path: 'components/motorgo-mini/example' target: esp32s3 - path: 'components/motorgo-plink/example' diff --git a/.github/workflows/upload_components.yml b/.github/workflows/upload_components.yml index 9c62f4b5c..bd5328a5c 100755 --- a/.github/workflows/upload_components.yml +++ b/.github/workflows/upload_components.yml @@ -92,6 +92,7 @@ jobs: components/max1704x components/mcp23x17 components/monitor + components/motorgo-axis components/motorgo-mini components/motorgo-plink components/mt6701 diff --git a/components/motorgo-axis/CMakeLists.txt b/components/motorgo-axis/CMakeLists.txt new file mode 100755 index 000000000..72c39bcec --- /dev/null +++ b/components/motorgo-axis/CMakeLists.txt @@ -0,0 +1,6 @@ +idf_component_register( + INCLUDE_DIRS "include" + SRC_DIRS "src" + REQUIRES base_component bldc_driver esp_driver_gpio i2c led lsm6dso math mt6701 spi task + REQUIRED_IDF_TARGETS "esp32s3" + ) diff --git a/components/motorgo-axis/README.md b/components/motorgo-axis/README.md new file mode 100755 index 000000000..d5c3a6318 --- /dev/null +++ b/components/motorgo-axis/README.md @@ -0,0 +1,31 @@ +# MotorGo Axis Board Support Package (BSP) Component + +[![Badge](https://components.espressif.com/components/espp/motorgo-axis/badge.svg)](https://components.espressif.com/components/espp/motorgo-axis) + +The MotorGo Axis is an ESP32-S3 motor-control board from Every Flavor +Robotics. It combines: + +- two 6-PWM BLDC motor outputs +- two MT6701-compatible SSI encoder chip-selects on a shared bus +- 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::MotorGoAxis` component provides a singleton board abstraction for +those documented pin mappings, plus convenient helpers for: + +- initializing the two BLDC motor driver channels with `espp::BldcDriver` +- initializing the shared encoder SPI/SSI bus and creating two + `espp::Mt6701` encoder instances +- initializing the onboard LSM6DS33 IMU on the hidden I2C bus via the shared + `espp::Lsm6dso` 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 Axis pin map, +starts the onboard LEDs breathing, initializes the onboard IMU, initializes the +two BLDC driver channels in a disabled state, and can optionally poll the two +encoder inputs. diff --git a/components/motorgo-axis/example/CMakeLists.txt b/components/motorgo-axis/example/CMakeLists.txt new file mode 100755 index 000000000..50ea15ca3 --- /dev/null +++ b/components/motorgo-axis/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-axis" + CACHE STRING + "List of components to include" + ) + +project(motorgo_axis_example) + +set(CMAKE_CXX_STANDARD 20) diff --git a/components/motorgo-axis/example/README.md b/components/motorgo-axis/example/README.md new file mode 100755 index 000000000..263125d37 --- /dev/null +++ b/components/motorgo-axis/example/README.md @@ -0,0 +1,47 @@ +# MotorGo Axis Example + +This example demonstrates how to use the `espp::MotorGoAxis` component to +initialize the hardware on the MotorGo Axis board. + +By default the example is intentionally safe: + +- it logs the documented motor, encoder, I2C, and LED pin mappings +- it starts the user and status LEDs breathing out of phase +- it initializes the two BLDC driver channels and then leaves them disabled + +Optional `menuconfig` flags let you also initialize and poll the two encoder +inputs. + +## How to use example + +### Hardware Required + +This example is designed for the MotorGo Axis board. + +The default configuration does not require motors or encoders to be connected. +If you enable encoder polling, connect the matching hardware first. + +### Configuration + +Open the example configuration menu if you want to enable encoder polling: + +```bash +idf.py menuconfig +``` + +Then open **MotorGo Axis Example Configuration** and enable: + +- **Enable encoder polling** + +### 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-axis/example/main/CMakeLists.txt b/components/motorgo-axis/example/main/CMakeLists.txt new file mode 100755 index 000000000..a941e22ba --- /dev/null +++ b/components/motorgo-axis/example/main/CMakeLists.txt @@ -0,0 +1,2 @@ +idf_component_register(SRC_DIRS "." + INCLUDE_DIRS ".") diff --git a/components/motorgo-axis/example/main/Kconfig.projbuild b/components/motorgo-axis/example/main/Kconfig.projbuild new file mode 100755 index 000000000..d77362d32 --- /dev/null +++ b/components/motorgo-axis/example/main/Kconfig.projbuild @@ -0,0 +1,7 @@ +menu "MotorGo Axis Example Configuration" + config MOTORGO_AXIS_EXAMPLE_ENABLE_ENCODER_POLLING + bool "Enable encoder polling" + default n + help + Initialize the two encoder inputs and log their measured angles. +endmenu diff --git a/components/motorgo-axis/example/main/motorgo_axis_example.cpp b/components/motorgo-axis/example/main/motorgo_axis_example.cpp new file mode 100755 index 000000000..6e21fba9c --- /dev/null +++ b/components/motorgo-axis/example/main/motorgo_axis_example.cpp @@ -0,0 +1,129 @@ +#include +#include +#include + +#include + +#include "logger.hpp" +#include "motorgo-axis.hpp" + +namespace { +static auto logger = espp::Logger({ + .tag = "MotorGoAxis Example", + .level = espp::Logger::Verbosity::INFO, +}); + +void log_board_info(espp::MotorGoAxis &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(); + + 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={}", enc_bus.sclk, enc_bus.miso); + logger.info("Encoder CS: [{}, {}]", encoder_cs[0], encoder_cs[1]); + for (size_t i = 0; i < espp::MotorGoAxis::num_motor_channels(); i++) { + auto pins = board.motor_pins(i); + logger.info("Motor {} pins: uh={}, ul={}, vh={}, vl={}, wh={}, wl={}", i + 1, pins.u_high, + pins.u_low, pins.v_high, pins.v_low, pins.w_high, pins.w_low); + } +} + +void log_imu_data(espp::MotorGoAxis &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_AXIS_EXAMPLE_ENABLE_ENCODER_POLLING +void log_encoder_angles(espp::MotorGoAxis &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}", angles[0], angles[1]); +} +#endif +} // namespace + +extern "C" void app_main() { + //! [motorgo-axis example] + auto &board = espp::MotorGoAxis::get(); + + if (!board.initialize_leds()) { + logger.warn("Failed to initialize indicator LEDs"); + } else { + board.start_led_breathing(); + } + + if (!board.initialize_motors()) { + logger.warn("Failed to initialize BLDC motor drivers"); + } else { + logger.info("Motor drivers initialized and left disabled"); + } + + if (!board.initialize_imu()) { + logger.warn("Failed to initialize onboard IMU"); + } else { + logger.info("IMU initialized"); + } + + log_board_info(board); + +#if CONFIG_MOTORGO_AXIS_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 + + while (true) { +#if CONFIG_MOTORGO_AXIS_EXAMPLE_ENABLE_ENCODER_POLLING + log_encoder_angles(board); +#endif + + log_imu_data(board); + + using namespace std::chrono_literals; + std::this_thread::sleep_for(500ms); + } + //! [motorgo-axis example] +} diff --git a/components/motorgo-axis/example/sdkconfig.defaults b/components/motorgo-axis/example/sdkconfig.defaults new file mode 100755 index 000000000..b01fdbeb4 --- /dev/null +++ b/components/motorgo-axis/example/sdkconfig.defaults @@ -0,0 +1 @@ +CONFIG_IDF_TARGET="esp32s3" diff --git a/components/motorgo-axis/idf_component.yml b/components/motorgo-axis/idf_component.yml new file mode 100755 index 000000000..f5f5cf3ea --- /dev/null +++ b/components/motorgo-axis/idf_component.yml @@ -0,0 +1,34 @@ +## IDF Component Manager Manifest File +license: "MIT" +description: "MotorGo Axis Board Support Package (BSP) component for ESP-IDF" +url: "https://github.com/esp-cpp/espp/tree/main/components/motorgo-axis" +repository: "git://github.com/esp-cpp/espp.git" +maintainers: + - William Emfinger +documentation: "https://esp-cpp.github.io/espp/motorgo_axis.html" +examples: + - path: example +tags: + - cpp + - Component + - ESP32S3 + - MotorGo + - Axis + - BSP + - BLDC + - Encoder + - IMU +dependencies: + idf: + version: '>=5.0' + espp/base_component: '>=1.0' + espp/bldc_driver: '>=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-axis/include/motorgo-axis.hpp b/components/motorgo-axis/include/motorgo-axis.hpp new file mode 100644 index 000000000..0f08a7317 --- /dev/null +++ b/components/motorgo-axis/include/motorgo-axis.hpp @@ -0,0 +1,380 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "base_component.hpp" +#include "bldc_driver.hpp" +#include "gaussian.hpp" +#include "i2c.hpp" +#include "led.hpp" +#include "lsm6dso.hpp" +#include "mt6701.hpp" +#include "spi.hpp" +#include "task.hpp" + +namespace espp { +/// MotorGoAxis is a lightweight board-support component for the MotorGo Axis +/// hardware. +/// +/// It exposes the documented pin mapping for the board's: +/// - two 6-PWM BLDC motor outputs +/// - two encoder chip-selects on a shared SSI/SPI bus +/// - one onboard LSM6DS3TR IMU on the hidden I2C bus +/// - Qwiic and hidden I2C buses +/// - user (green) and status (blue) LEDs +/// +/// The class is a singleton and can be accessed with get(). +/// +/// \section motorgo_axis_ex1 MotorGo Axis Example +/// \snippet motorgo_axis_example.cpp motorgo-axis example +class MotorGoAxis : public BaseComponent { +public: + /// Alias for the SSI-based magnetic encoder helper used on each motor + /// channel. + using Encoder = espp::Mt6701; + /// Alias for the 6-PWM BLDC motor driver helper used on each motor channel. + using MotorDriver = espp::BldcDriver; + /// Alias for the onboard hidden-bus IMU helper. + using Imu = espp::Lsm6dso; + + /// Pin mapping for one BLDC motor channel. + struct MotorPins { + gpio_num_t u_high{GPIO_NUM_NC}; ///< Phase U high-side PWM pin. + gpio_num_t u_low{GPIO_NUM_NC}; ///< Phase U low-side PWM pin. + gpio_num_t v_high{GPIO_NUM_NC}; ///< Phase V high-side PWM pin. + gpio_num_t v_low{GPIO_NUM_NC}; ///< Phase V low-side PWM pin. + gpio_num_t w_high{GPIO_NUM_NC}; ///< Phase W high-side PWM pin. + gpio_num_t w_low{GPIO_NUM_NC}; ///< Phase W low-side PWM pin. + }; + + /// 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. + }; + + /// 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 MotorGoAxis object. + static MotorGoAxis &get() { + static MotorGoAxis instance; + return instance; + } + + /// Deleted copy constructor. + MotorGoAxis(const MotorGoAxis &) = delete; + /// Deleted copy assignment operator. + MotorGoAxis &operator=(const MotorGoAxis &) = delete; + /// Deleted move constructor. + MotorGoAxis(MotorGoAxis &&) = delete; + /// Deleted move assignment operator. + MotorGoAxis &operator=(MotorGoAxis &&) = 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 both channels. + /// \return Array containing the six 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 and MISO pins used for the encoder bus. + EncoderBusPins encoder_bus_pins() const; + + /// Get the two 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 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 two BLDC motor driver helpers. + /// \param power_supply_voltage Maximum motor supply voltage in volts. + /// \param limit_voltage Optional motor voltage limit in volts. Values less + /// than zero mean "no extra limit" and will be clamped + /// to the supply voltage by `espp::BldcDriver`. + /// \param dead_zone_ns Dead-time to apply to both sides of each phase PWM. + /// \return True if both motor driver helpers were initialized successfully; + /// false if the configuration is invalid or any channel could not be + /// configured. + bool initialize_motors(float power_supply_voltage = driver_default_power_supply_voltage(), + float limit_voltage = driver_default_voltage_limit(), + uint64_t dead_zone_ns = default_motor_dead_zone_ns()); + + /// 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; + + /// Check whether one motor driver helper is currently enabled. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return True if the requested motor driver exists and is enabled. + bool motor_driver_enabled(size_t index) const; + + /// Enable one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + /// \return True if the driver exists and was enabled. + bool enable_motor_driver(size_t index); + + /// Disable one motor driver helper. + /// \param index Zero-based motor index in the range [0, num_motor_channels()). + void disable_motor_driver(size_t index); + + /// Enable all initialized motor driver helpers. + void enable_all_motor_drivers(); + + /// Disable all initialized motor driver helpers. + void disable_all_motor_drivers(); + + /// Initialize the shared encoder bus and create the two 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 both 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 or the + /// LEDs could not be driven after setup. + 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 default motor supply voltage used for the board definition. + /// \return Default power supply voltage in volts. + static constexpr float driver_default_power_supply_voltage() { + return driver_default_power_supply_voltage_; + } + /// Get the default motor voltage limit used for the board definition. + /// \return Default motor voltage limit in volts. + static constexpr float driver_default_voltage_limit() { return driver_default_voltage_limit_; } + /// Get the default BLDC driver dead-time. + /// \return Default dead-time in nanoseconds. + static constexpr uint64_t default_motor_dead_zone_ns() { return default_motor_dead_zone_ns_; } + /// 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: + MotorGoAxis(); + + bool initialize_encoder_spi(); + bool read_encoder(size_t index, uint8_t *data, size_t size); + 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_{{ + {.u_high = GPIO_NUM_3, + .u_low = GPIO_NUM_17, + .v_high = GPIO_NUM_8, + .v_low = GPIO_NUM_16, + .w_high = GPIO_NUM_18, + .w_low = GPIO_NUM_15}, + {.u_high = GPIO_NUM_41, + .u_low = GPIO_NUM_11, + .v_high = GPIO_NUM_42, + .v_low = GPIO_NUM_10, + .w_high = GPIO_NUM_12, + .w_low = GPIO_NUM_9}, + }}; + static constexpr std::array encoder_cs_pin_map_{ + GPIO_NUM_6, + GPIO_NUM_7, + }; + static constexpr EncoderBusPins encoder_bus_pin_map_{.sclk = GPIO_NUM_5, .miso = GPIO_NUM_4}; + 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_43; + static constexpr gpio_num_t status_led_pin_ = GPIO_NUM_44; + 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 float driver_default_power_supply_voltage_ = 17.0f; + static constexpr float driver_default_voltage_limit_ = 17.0f; + static constexpr uint64_t default_motor_dead_zone_ns_ = 100; + 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, 2> encoder_spi_devices_{}; + std::array, 2> encoders_{}; + std::shared_ptr> imu_i2c_device_; + std::shared_ptr imu_; + + 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::array, 2> motor_drivers_{}; + std::shared_ptr indicator_leds_; + 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 MotorGoAxis +} // namespace espp diff --git a/components/motorgo-axis/src/motorgo-axis.cpp b/components/motorgo-axis/src/motorgo-axis.cpp new file mode 100755 index 000000000..dcefc6cfb --- /dev/null +++ b/components/motorgo-axis/src/motorgo-axis.cpp @@ -0,0 +1,403 @@ +#include "motorgo-axis.hpp" + +#include +#include +#include +#include +#include + +using namespace espp; + +MotorGoAxis::MotorGoAxis() + : BaseComponent("MotorGoAxis") {} + +I2c &MotorGoAxis::get_external_i2c() { return qwiic_i2c_; } + +I2c &MotorGoAxis::qwiic_i2c() { return qwiic_i2c_; } + +I2c &MotorGoAxis::hidden_i2c() { return hidden_i2c_; } + +std::array MotorGoAxis::motor_pins() const { return motor_pin_map_; } + +MotorGoAxis::MotorPins MotorGoAxis::motor_pins(size_t index) const { + if (index >= motor_pin_map_.size()) { + logger_.error("Invalid motor index: {}", index); + return {}; + } + return motor_pin_map_[index]; +} + +MotorGoAxis::EncoderBusPins MotorGoAxis::encoder_bus_pins() const { return encoder_bus_pin_map_; } + +std::array MotorGoAxis::encoder_cs_pins() const { return encoder_cs_pin_map_; } + +gpio_num_t MotorGoAxis::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]; +} + +MotorGoAxis::I2cPins MotorGoAxis::qwiic_pins() const { return qwiic_pin_map_; } + +MotorGoAxis::I2cPins MotorGoAxis::hidden_i2c_pins() const { return hidden_i2c_pin_map_; } + +MotorGoAxis::LedPins MotorGoAxis::led_pins() const { + return {.user = user_led_pin_, .status = status_led_pin_}; +} + +bool MotorGoAxis::initialize_motors(float power_supply_voltage, float limit_voltage, + uint64_t dead_zone_ns) { + 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 (power_supply_voltage <= 0.0f) { + logger_.error("Motor power supply voltage must be greater than zero"); + return false; + } + 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_h = pins.u_high, + .gpio_a_l = pins.u_low, + .gpio_b_h = pins.v_high, + .gpio_b_l = pins.v_low, + .gpio_c_h = pins.w_high, + .gpio_c_l = pins.w_low, + .gpio_enable = -1, + .gpio_fault = -1, + .power_supply_voltage = power_supply_voltage, + .limit_voltage = limit_voltage, + .dead_zone_ns = dead_zone_ns, + .log_level = get_log_level(), + }); + motor_drivers_[i] = driver; + } + disable_all_motor_drivers(); + return true; +} + +std::shared_ptr MotorGoAxis::motor_driver(size_t index) const { + if (index >= motor_drivers_.size()) { + logger_.error("Invalid motor index: {}", index); + return nullptr; + } + return motor_drivers_[index]; +} + +bool MotorGoAxis::motor_driver_enabled(size_t index) const { + auto driver = motor_driver(index); + return driver && driver->is_enabled(); +} + +bool MotorGoAxis::enable_motor_driver(size_t index) { + auto driver = motor_driver(index); + if (!driver) { + logger_.error("Motor driver {} not initialized", index + 1); + return false; + } + driver->enable(); + return driver->is_enabled(); +} + +void MotorGoAxis::disable_motor_driver(size_t index) { + auto driver = motor_driver(index); + if (!driver) { + logger_.error("Motor driver {} not initialized", index + 1); + return; + } + driver->disable(); +} + +void MotorGoAxis::enable_all_motor_drivers() { + for (auto &driver : motor_drivers_) { + if (driver) { + driver->enable(); + } + } +} + +void MotorGoAxis::disable_all_motor_drivers() { + for (auto &driver : motor_drivers_) { + if (driver) { + driver->disable(); + } + } +} + +bool MotorGoAxis::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 MotorGoAxis::encoder(size_t index) { + if (index >= encoders_.size()) { + logger_.error("Invalid encoder index: {}", index); + return nullptr; + } + return encoders_[index]; +} + +void MotorGoAxis::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 MotorGoAxis::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 = 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; +} + +std::shared_ptr MotorGoAxis::imu() const { return imu_; } + +bool MotorGoAxis::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(&MotorGoAxis::led_task_callback, this, _1, _2, _3), + .task_config = {.name = "motorgo_axis_led"}}); + 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; +} + +void MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::get_led_breathing_period() { return breathing_period_; } + +std::shared_ptr MotorGoAxis::leds() { return indicator_leds_; } + +espp::Gaussian &MotorGoAxis::gaussian() { return gaussian_; } + +bool MotorGoAxis::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 = GPIO_NUM_NC, + .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()); + encoder_spi_devices_ = {}; + encoder_spi_.reset(); + return false; + } + } + return true; +} + +bool IRAM_ATTR MotorGoAxis::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 MotorGoAxis::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 MotorGoAxis::led_task_callback(std::mutex &m, std::condition_variable &cv, + bool &task_notified) { + using namespace std::chrono_literals; + 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; + return false; +} diff --git a/doc/Doxyfile b/doc/Doxyfile index c739531b3..47901f4ce 100755 --- a/doc/Doxyfile +++ b/doc/Doxyfile @@ -126,6 +126,7 @@ EXAMPLE_PATH = \ $(PROJECT_PATH)/components/matouch-rotary-display/example/main/matouch_rotary_display_example.cpp \ $(PROJECT_PATH)/components/max1704x/example/main/max1704x_example.cpp \ $(PROJECT_PATH)/components/monitor/example/main/monitor_example.cpp \ + $(PROJECT_PATH)/components/motorgo-axis/example/main/motorgo_axis_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 \ @@ -301,6 +302,7 @@ INPUT = \ $(PROJECT_PATH)/components/max1704x/include/max1704x.hpp \ $(PROJECT_PATH)/components/monitor/include/heap_monitor.hpp \ $(PROJECT_PATH)/components/monitor/include/task_monitor.hpp \ + $(PROJECT_PATH)/components/motorgo-axis/include/motorgo-axis.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 \ diff --git a/doc/en/index.rst b/doc/en/index.rst index ff9c77cca..1736e6170 100755 --- a/doc/en/index.rst +++ b/doc/en/index.rst @@ -50,6 +50,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 + ``espp::Lsm6dso`` helper +- controlling the user and status LEDs with direct brightness setters or a + Gaussian breathing effect + +.. ------------------------------- Example ------------------------------------- + +.. toctree:: + + motorgo_axis_example.md + +.. ---------------------------- API Reference ---------------------------------- + +API Reference +------------- + +.. include-build-file:: inc/motorgo-axis.inc diff --git a/doc/en/motorgo_axis_example.md b/doc/en/motorgo_axis_example.md new file mode 100755 index 000000000..0e4283f95 --- /dev/null +++ b/doc/en/motorgo_axis_example.md @@ -0,0 +1,2 @@ +```{include} ../../components/motorgo-axis/example/README.md +```