diff --git a/andino_firmware/platformio.ini b/andino_firmware/platformio.ini index 6283cef..7857b86 100644 --- a/andino_firmware/platformio.ini +++ b/andino_firmware/platformio.ini @@ -29,6 +29,19 @@ lib_deps = adafruit/Adafruit BusIO adafruit/Adafruit Unified Sensor +; Base configuration for Espressif ESP32 based boards. +[base_espressif32] +platform = espressif32 +framework = arduino +monitor_speed = 57600 +test_ignore = desktop/* +lib_deps = + Wire + SPI + adafruit/Adafruit BNO055 + adafruit/Adafruit BusIO + adafruit/Adafruit Unified Sensor + ; Base configuration for desktop platforms (for unit testing). [base_desktop] platform = native @@ -53,3 +66,8 @@ board = nanoatmega328 ; Environment for desktop platforms (Windows, macOS, Linux, etc). [env:desktop] extends = base_build, base_desktop + +; Environment for ESP32 DevKit boards. +[env:esp32dev] +extends = base_build, base_espressif32 +board = esp32dev diff --git a/andino_firmware/src/andino_firmware.ino b/andino_firmware/src/andino_firmware.ino new file mode 100644 index 0000000..6ad0506 --- /dev/null +++ b/andino_firmware/src/andino_firmware.ino @@ -0,0 +1,16 @@ +/* + * Arduino IDE / Arduino-core entry points. + * + * This sketch file provides the standard `setup()` and `loop()` symbols that + * the Arduino core (including ESP32) expects, and simply forwards them to the + * existing andino::App implementation. + * + * NOTE: Do not put any application logic here; keep everything in App to + * ensure the serial protocol and higher-level behavior remain unchanged. + */ + +#include "app.h" + +void setup() { andino::App::setup(); } + +void loop() { andino::App::loop(); } diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index 3cc298b..34c6a72 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -75,13 +75,19 @@ #include "digital_out_arduino.h" #include "encoder.h" #include "hw.h" -#include "interrupt_in_arduino.h" #include "motor.h" #include "pid.h" -#include "pwm_out_arduino.h" #include "serial_stream_arduino.h" #include "shell.h" +#if defined(ARDUINO_ARCH_ESP32) +#include "interrupt_in_esp32.h" +#include "pwm_out_esp32_mcpwm.h" +#else +#include "interrupt_in_arduino.h" +#include "pwm_out_arduino.h" +#endif + namespace andino { SerialStreamArduino App::serial_stream_; @@ -89,24 +95,48 @@ SerialStreamArduino App::serial_stream_; Shell App::shell_; DigitalOutArduino App::left_motor_enable_digital_out_(Hw::kLeftMotorEnableGpioPin); +#if defined(ARDUINO_ARCH_ESP32) +PwmOutEsp32Mcpwm App::left_motor_forward_pwm_out_(Hw::kLeftMotorForwardGpioPin, MCPWM_UNIT_0, + MCPWM0A); +PwmOutEsp32Mcpwm App::left_motor_backward_pwm_out_(Hw::kLeftMotorBackwardGpioPin, MCPWM_UNIT_0, + MCPWM0B); +#else PwmOutArduino App::left_motor_forward_pwm_out_(Hw::kLeftMotorForwardGpioPin); PwmOutArduino App::left_motor_backward_pwm_out_(Hw::kLeftMotorBackwardGpioPin); +#endif Motor App::left_motor_(&left_motor_enable_digital_out_, &left_motor_forward_pwm_out_, &left_motor_backward_pwm_out_); DigitalOutArduino App::right_motor_enable_digital_out_(Hw::kRightMotorEnableGpioPin); +#if defined(ARDUINO_ARCH_ESP32) +PwmOutEsp32Mcpwm App::right_motor_forward_pwm_out_(Hw::kRightMotorForwardGpioPin, MCPWM_UNIT_1, + MCPWM1A); +PwmOutEsp32Mcpwm App::right_motor_backward_pwm_out_(Hw::kRightMotorBackwardGpioPin, MCPWM_UNIT_1, + MCPWM1B); +#else PwmOutArduino App::right_motor_forward_pwm_out_(Hw::kRightMotorForwardGpioPin); PwmOutArduino App::right_motor_backward_pwm_out_(Hw::kRightMotorBackwardGpioPin); +#endif Motor App::right_motor_(&right_motor_enable_digital_out_, &right_motor_forward_pwm_out_, &right_motor_backward_pwm_out_); +#if defined(ARDUINO_ARCH_ESP32) +InterruptInEsp32 App::left_encoder_channel_a_interrupt_in_(Hw::kLeftEncoderChannelAGpioPin); +InterruptInEsp32 App::left_encoder_channel_b_interrupt_in_(Hw::kLeftEncoderChannelBGpioPin); +#else InterruptInArduino App::left_encoder_channel_a_interrupt_in_(Hw::kLeftEncoderChannelAGpioPin); InterruptInArduino App::left_encoder_channel_b_interrupt_in_(Hw::kLeftEncoderChannelBGpioPin); +#endif Encoder App::left_encoder_(&left_encoder_channel_a_interrupt_in_, &left_encoder_channel_b_interrupt_in_); +#if defined(ARDUINO_ARCH_ESP32) +InterruptInEsp32 App::right_encoder_channel_a_interrupt_in_(Hw::kRightEncoderChannelAGpioPin); +InterruptInEsp32 App::right_encoder_channel_b_interrupt_in_(Hw::kRightEncoderChannelBGpioPin); +#else InterruptInArduino App::right_encoder_channel_a_interrupt_in_(Hw::kRightEncoderChannelAGpioPin); InterruptInArduino App::right_encoder_channel_b_interrupt_in_(Hw::kRightEncoderChannelBGpioPin); +#endif Encoder App::right_encoder_(&right_encoder_channel_a_interrupt_in_, &right_encoder_channel_b_interrupt_in_); @@ -187,6 +217,7 @@ void App::adjust_motors_speed() { int right_motor_speed = 0; left_pid_controller_.compute(left_encoder_.read(), left_motor_speed); right_pid_controller_.compute(right_encoder_.read(), right_motor_speed); + if (left_pid_controller_.enabled()) { left_motor_.set_speed(left_motor_speed); } @@ -281,41 +312,43 @@ void App::cmd_set_motors_pwm_cb(int argc, char** argv) { // Reset the auto stop timer. last_set_motors_speed_cmd_ = millis(); - left_motor_.set_speed(left_motor_pwm); right_motor_.set_speed(right_motor_pwm); Serial.println("OK"); } void App::cmd_set_pid_tuning_gains_cb(int argc, char** argv) { - // TODO(jballoffet): Refactor to expect command multiple arguments. - if (argc < 2) { + if (argc < 5) { return; } - static constexpr int kSizePidArgs{4}; - int i = 0; - char arg[20]; - char* str; - int pid_args[kSizePidArgs]{0, 0, 0, 0}; - - // Example: "u 30:20:10:50". - strcpy(arg, argv[1]); - char* p = arg; - while ((str = strtok_r(p, ":", &p)) != NULL && i < kSizePidArgs) { - pid_args[i] = atoi(str); - i++; + int kp = 0; + int kd = 0; + int ki = 0; + int ko = 0; + + // Space-separated format: "u 30 10 0 10". + kp = atoi(argv[1]); + kd = atoi(argv[2]); + ki = atoi(argv[3]); + ko = atoi(argv[4]); + + + // Prevent invalid configuration that would cause divide-by-zero inside the PID. + if (ko == 0) { + ko = 1; } - left_pid_controller_.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); - right_pid_controller_.set_tunings(pid_args[0], pid_args[1], pid_args[2], pid_args[3]); + + left_pid_controller_.set_tunings(kp, kd, ki, ko); + right_pid_controller_.set_tunings(kp, kd, ki, ko); Serial.print("PID Updated: "); - Serial.print(pid_args[0]); + Serial.print(kp); Serial.print(" "); - Serial.print(pid_args[1]); + Serial.print(kd); Serial.print(" "); - Serial.print(pid_args[2]); + Serial.print(ki); Serial.print(" "); - Serial.println(pid_args[3]); + Serial.println(ko); Serial.println("OK"); } diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index 0bcbef8..3576733 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -33,13 +33,19 @@ #include "digital_out_arduino.h" #include "encoder.h" -#include "interrupt_in_arduino.h" #include "motor.h" #include "pid.h" -#include "pwm_out_arduino.h" #include "serial_stream_arduino.h" #include "shell.h" +#if defined(ARDUINO_ARCH_ESP32) +#include "interrupt_in_esp32.h" +#include "pwm_out_esp32_mcpwm.h" +#else +#include "interrupt_in_arduino.h" +#include "pwm_out_arduino.h" +#endif + namespace andino { /// @brief This class wraps the MCU main application. @@ -99,24 +105,44 @@ class App { /// Left wheel motor. static DigitalOutArduino left_motor_enable_digital_out_; +#if defined(ARDUINO_ARCH_ESP32) + static PwmOutEsp32Mcpwm left_motor_forward_pwm_out_; + static PwmOutEsp32Mcpwm left_motor_backward_pwm_out_; +#else static PwmOutArduino left_motor_forward_pwm_out_; static PwmOutArduino left_motor_backward_pwm_out_; +#endif static Motor left_motor_; /// Right wheel motor. static DigitalOutArduino right_motor_enable_digital_out_; +#if defined(ARDUINO_ARCH_ESP32) + static PwmOutEsp32Mcpwm right_motor_forward_pwm_out_; + static PwmOutEsp32Mcpwm right_motor_backward_pwm_out_; +#else static PwmOutArduino right_motor_forward_pwm_out_; static PwmOutArduino right_motor_backward_pwm_out_; +#endif static Motor right_motor_; /// Left wheel encoder. +#if defined(ARDUINO_ARCH_ESP32) + static InterruptInEsp32 left_encoder_channel_a_interrupt_in_; + static InterruptInEsp32 left_encoder_channel_b_interrupt_in_; +#else static InterruptInArduino left_encoder_channel_a_interrupt_in_; static InterruptInArduino left_encoder_channel_b_interrupt_in_; +#endif static Encoder left_encoder_; /// Right wheel encoder. +#if defined(ARDUINO_ARCH_ESP32) + static InterruptInEsp32 right_encoder_channel_a_interrupt_in_; + static InterruptInEsp32 right_encoder_channel_b_interrupt_in_; +#else static InterruptInArduino right_encoder_channel_a_interrupt_in_; static InterruptInArduino right_encoder_channel_b_interrupt_in_; +#endif static Encoder right_encoder_; /// PID controllers (one per wheel). diff --git a/andino_firmware/src/constants.h b/andino_firmware/src/constants.h index c4f4d31..8592bde 100644 --- a/andino_firmware/src/constants.h +++ b/andino_firmware/src/constants.h @@ -36,6 +36,10 @@ struct Constants { /// @brief Serial port baud rate. static constexpr long kBaudrate{57600}; + // @brief ESP32 MCPWM frequency (Hz). This is the frequency of the PWM signal generated by the ESP32's MCPWM peripheral, which controls the motors. A common choice for motor control is around + // 20 kHz, which is above the audible range and can help reduce noise. Adjust as needed for your specific motors and application. + static constexpr int kMcpwmFrequency{20000}; + /// @brief Time window to automatically stop the robot if no command has been received [ms]. static constexpr long kAutoStopWindow{3000}; diff --git a/andino_firmware/src/hw.h b/andino_firmware/src/hw.h index 850525a..0bf7949 100644 --- a/andino_firmware/src/hw.h +++ b/andino_firmware/src/hw.h @@ -33,6 +33,38 @@ namespace andino { /// @brief Hardware configuration. struct Hw { +#if defined(ARDUINO_ARCH_ESP32) + /// @brief Left encoder channel A pin. + /// @note These pins are chosen for a typical ESP32 DevKit; adjust to match + /// your wiring. + static constexpr int kLeftEncoderChannelAGpioPin{33}; + /// @brief Left encoder channel B pin. + static constexpr int kLeftEncoderChannelBGpioPin{32}; + + /// @brief Right encoder channel A pin. + static constexpr int kRightEncoderChannelAGpioPin{34}; + /// @brief Right encoder channel B pin. + static constexpr int kRightEncoderChannelBGpioPin{35}; + + /// @brief Left motor driver backward pin (L298N IN3). + static constexpr int kLeftMotorBackwardGpioPin{27}; + /// @brief Left motor driver forward pin (L298N IN4). + static constexpr int kLeftMotorForwardGpioPin{26}; + /// @brief Left motor driver enable pin (L298N ENB). + static constexpr int kLeftMotorEnableGpioPin{25}; + + /// @brief Right motor driver backward pin (L298N IN1). + static constexpr int kRightMotorBackwardGpioPin{12}; + /// @brief Right motor driver forward pin (L298N IN2). + static constexpr int kRightMotorForwardGpioPin{14}; + /// @brief Right motor driver enable pin (L298N ENA). + static constexpr int kRightMotorEnableGpioPin{13}; + + /// @brief IMU sensor I2C SCL pin (default ESP32 SCL). + static constexpr int kImuI2cSclPin{22}; + /// @brief IMU sensor I2C SDA pin (default ESP32 SDA). + static constexpr int kImuI2cSdaPin{21}; +#else /// @brief Left encoder channel A pin. Connected to PD2 (digital pin 2). static constexpr int kLeftEncoderChannelAGpioPin{2}; /// @brief Left encoder channel B pin. Connected to PD3 (digital pin 3). @@ -65,6 +97,7 @@ struct Hw { static constexpr int kImuI2cSclPin{19}; /// @brief IMU sensor I2C SDA pin. Connected to PC4 (digital pin 18, analog pin A4). static constexpr int kImuI2cSdaPin{18}; +#endif }; } // namespace andino diff --git a/andino_firmware/src/interrupt_in_arduino.cpp b/andino_firmware/src/interrupt_in_arduino.cpp index 4e31e0d..c4a4da1 100644 --- a/andino_firmware/src/interrupt_in_arduino.cpp +++ b/andino_firmware/src/interrupt_in_arduino.cpp @@ -31,6 +31,10 @@ #include +// This implementation is AVR-specific and must not be compiled on other +// architectures (e.g. ESP32), where a different interrupt backend is used. +#if !defined(ARDUINO_ARCH_ESP32) + /// Holds the attached callbacks. static andino::InterruptIn::InterruptCallback g_callbacks[3] = {nullptr}; @@ -86,3 +90,5 @@ void InterruptInArduino::attach(InterruptCallback callback) const { } } // namespace andino + +#endif // !ARDUINO_ARCH_ESP32 diff --git a/andino_firmware/src/interrupt_in_esp32.cpp b/andino_firmware/src/interrupt_in_esp32.cpp new file mode 100644 index 0000000..888b635 --- /dev/null +++ b/andino_firmware/src/interrupt_in_esp32.cpp @@ -0,0 +1,48 @@ +// BSD 3-Clause License +// +// Copyright (c) 2026, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "interrupt_in_esp32.h" + +#if defined(ARDUINO_ARCH_ESP32) + +#include + +namespace andino { + +void InterruptInEsp32::begin() const { pinMode(gpio_pin_, INPUT_PULLUP); } + +int InterruptInEsp32::read() const { return digitalRead(gpio_pin_); } + +void InterruptInEsp32::attach(InterruptCallback callback) const { + attachInterrupt(digitalPinToInterrupt(gpio_pin_), callback, CHANGE); +} + +} // namespace andino + +#endif // ARDUINO_ARCH_ESP32 diff --git a/andino_firmware/src/interrupt_in_esp32.h b/andino_firmware/src/interrupt_in_esp32.h new file mode 100644 index 0000000..9d028ac --- /dev/null +++ b/andino_firmware/src/interrupt_in_esp32.h @@ -0,0 +1,58 @@ +// BSD 3-Clause License +// +// Copyright (c) 2026, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#if defined(ARDUINO_ARCH_ESP32) + +#include "interrupt_in.h" + +namespace andino { + +/// @brief ESP32 implementation of the digital interrupt input interface. +/// +/// This relies on attachInterrupt / digitalPinToInterrupt, which is supported +/// by the Arduino-ESP32 core. +class InterruptInEsp32 : public InterruptIn { + public: + /// @brief Constructs an InterruptInEsp32 using the specified GPIO pin. + /// + /// @param gpio_pin GPIO pin. + explicit InterruptInEsp32(const int gpio_pin) : InterruptIn(gpio_pin) {} + + void begin() const override; + + int read() const override; + + void attach(InterruptCallback callback) const override; +}; + +} // namespace andino + +#endif // ARDUINO_ARCH_ESP32 diff --git a/andino_firmware/src/pid.cpp b/andino_firmware/src/pid.cpp index 476b8f4..c775e25 100644 --- a/andino_firmware/src/pid.cpp +++ b/andino_firmware/src/pid.cpp @@ -105,18 +105,35 @@ void Pid::compute(int encoder_count, int& computed_output) { long output = (kp_ * error - kd_ * (input - last_input_) + integral_term_) / ko_; output += last_output_; - // Accumulate integral term as long as output doesn't saturate. + // Saturate output to configured limits. Also avoid integral wind-up when saturated. + bool saturated = false; + if (output >= output_max_) { output = output_max_; + saturated = true; } else if (output <= output_min_) { output = output_min_; - } else { + saturated = true; + } + + // Accumulate integral term only when not saturated. + if (!saturated) { integral_term_ += ki_ * error; } - // Set the computed output accordingly. + // Start from the saturated output, then apply a directional clamp only + // to the value returned to the caller. Internal state (last_output_) + // keeps the full saturated output. computed_output = output; + // If the setpoint is positive, do not command negative output (reverse). + // If the setpoint is negative, do not command positive output (forward). + if (setpoint_ > 0 && computed_output < 0) { + computed_output = 0; + } else if (setpoint_ < 0 && computed_output > 0) { + computed_output = 0; + } + // Store obtained values. last_encoder_count_ = encoder_count; last_input_ = input; @@ -129,7 +146,12 @@ void Pid::set_tunings(int kp, int kd, int ki, int ko) { kp_ = kp; kd_ = kd; ki_ = ki; - ko_ = ko; + // Avoid invalid configuration that would cause divide-by-zero in compute(). + if (ko == 0) { + ko_ = 1; + } else { + ko_ = ko; + } } } // namespace andino diff --git a/andino_firmware/src/pwm_out_esp32_mcpwm.cpp b/andino_firmware/src/pwm_out_esp32_mcpwm.cpp new file mode 100644 index 0000000..2e53f4a --- /dev/null +++ b/andino_firmware/src/pwm_out_esp32_mcpwm.cpp @@ -0,0 +1,118 @@ +// BSD 3-Clause License +// +// Copyright (c) 2026, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#include "pwm_out_esp32_mcpwm.h" + +#if defined(ARDUINO_ARCH_ESP32) + +#include +#include + +#include "driver/mcpwm.h" + +namespace andino { + +namespace { + +mcpwm_timer_t timer_from_signal(mcpwm_io_signals_t signal) { + switch (signal) { + case MCPWM0A: + case MCPWM0B: + return MCPWM_TIMER_0; + case MCPWM1A: + case MCPWM1B: + return MCPWM_TIMER_1; + case MCPWM2A: + case MCPWM2B: + return MCPWM_TIMER_2; + default: + return MCPWM_TIMER_0; + } +} + +mcpwm_operator_t operator_from_signal(mcpwm_io_signals_t signal) { + switch (signal) { + case MCPWM0A: + case MCPWM1A: + case MCPWM2A: + return MCPWM_OPR_A; + case MCPWM0B: + case MCPWM1B: + case MCPWM2B: + return MCPWM_OPR_B; + default: + return MCPWM_OPR_A; + } +} + +// Track whether a given (unit, timer) pair has already been initialized so we +// don't call mcpwm_init more than once per timer. +bool initialized[MCPWM_UNIT_MAX][MCPWM_TIMER_MAX] = {{false}}; + +} // namespace + +void PwmOutEsp32Mcpwm::begin() const { + const mcpwm_timer_t timer = timer_from_signal(io_signal_); + + // Attach the MCPWM output to the selected pin. + mcpwm_gpio_init(unit_, io_signal_, gpio_pin_); + + if (!initialized[unit_][timer]) { + mcpwm_config_t config; + config.frequency = Constants::kMcpwmFrequency; + config.cmpr_a = 0.0f; + config.cmpr_b = 0.0f; + config.counter_mode = MCPWM_UP_COUNTER; + config.duty_mode = MCPWM_DUTY_MODE_0; + + mcpwm_init(unit_, timer, &config); + initialized[unit_][timer] = true; + } + + // Start with the output stopped. + write(0); +} + +void PwmOutEsp32Mcpwm::write(int value) const { + if (value < 0) { + value = 0; + } else if (value > 255) { + value = 255; + } + + const float duty = (static_cast(value) * 100.0f) / 255.0f; + const mcpwm_timer_t timer = timer_from_signal(io_signal_); + const mcpwm_operator_t op = operator_from_signal(io_signal_); + + mcpwm_set_duty(unit_, timer, op, duty); +} + +} // namespace andino + +#endif // ARDUINO_ARCH_ESP32 diff --git a/andino_firmware/src/pwm_out_esp32_mcpwm.h b/andino_firmware/src/pwm_out_esp32_mcpwm.h new file mode 100644 index 0000000..662dbf8 --- /dev/null +++ b/andino_firmware/src/pwm_out_esp32_mcpwm.h @@ -0,0 +1,70 @@ +// BSD 3-Clause License +// +// Copyright (c) 2026, Ekumen Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// 1. Redistributions of source code must retain the above copyright notice, this +// list of conditions and the following disclaimer. +// +// 2. Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// 3. Neither the name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +// FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +// DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +// OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +#pragma once + +#if defined(ARDUINO_ARCH_ESP32) + +#include "pwm_out.h" + +#include "driver/mcpwm.h" + +namespace andino { + +/// @brief ESP32 implementation of the PWM output interface using MCPWM. +/// +/// This maps the logical 0-255 value used in the firmware to a 0-100% duty +/// cycle on the selected MCPWM unit/timer/operator, similar to the example +/// code you provided. +class PwmOutEsp32Mcpwm : public PwmOut { + public: + /// @brief Constructs a PwmOutEsp32Mcpwm using the specified GPIO pin, + /// MCPWM unit and IO signal. + /// + /// @param gpio_pin GPIO pin. + /// @param unit MCPWM unit (e.g. MCPWM_UNIT_0). + /// @param io_signal MCPWM IO signal (e.g. MCPWM0A, MCPWM0B, ...). + PwmOutEsp32Mcpwm(const int gpio_pin, mcpwm_unit_t unit, mcpwm_io_signals_t io_signal) + : PwmOut(gpio_pin), unit_(unit), io_signal_(io_signal) {} + + void begin() const override; + + void write(int value) const override; + + private: + /// MCPWM unit used by this PWM output. + const mcpwm_unit_t unit_; + + /// MCPWM IO signal used by this PWM output. + const mcpwm_io_signals_t io_signal_; +}; + +} // namespace andino + +#endif // ARDUINO_ARCH_ESP32