From a242ce08469ae683ed3d7fe1f119e297105289fe Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Sun, 20 Jul 2025 18:14:40 -0400 Subject: [PATCH] DC Motor --- .../constants/include/constants/module.h | 5 + main/control/ActuatorFactory.cpp | 11 +- main/control/DCMotorActuator.cpp | 151 ++++++++++++++++++ main/control/Servo1Actuator.cpp | 15 +- main/include/control/DCMotorActuator.h | 28 ++++ main/main.cpp | 3 +- 6 files changed, 201 insertions(+), 12 deletions(-) create mode 100644 main/control/DCMotorActuator.cpp create mode 100644 main/include/control/DCMotorActuator.h diff --git a/components/constants/include/constants/module.h b/components/constants/include/constants/module.h index 406d95a..3ad49a6 100644 --- a/components/constants/include/constants/module.h +++ b/components/constants/include/constants/module.h @@ -15,4 +15,9 @@ inline std::unordered_map MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLI #define SERVO_GPIO 1 +#define DC_MOTOR_PWM_FWD 1 +#define DC_MOTOR_PWM_REV 2 +#define DC_ENCODER_A 15 +#define DC_ENCODER_B 16 + #endif //MODULE_H diff --git a/main/control/ActuatorFactory.cpp b/main/control/ActuatorFactory.cpp index 9fb673c..989fb6a 100644 --- a/main/control/ActuatorFactory.cpp +++ b/main/control/ActuatorFactory.cpp @@ -2,14 +2,21 @@ // Created by Johnathon Slightham on 2025-07-15. // +#include + #include "control/ActuatorFactory.h" -#include +#include "control/DCMotorActuator.h" +#include "control/Servo1Actuator.h" -std::unique_ptr ActuatorFactory::create_actuator(ModuleType type) { +#include "flatbuffers_generated/RobotModule_generated.h" + +std::unique_ptr ActuatorFactory::create_actuator(const ModuleType type) { switch (type) { case ModuleType_SERVO_1: return std::make_unique(); + case ModuleType_DC_MOTOR: + return std::make_unique(); default: return nullptr; } diff --git a/main/control/DCMotorActuator.cpp b/main/control/DCMotorActuator.cpp new file mode 100644 index 0000000..22340ac --- /dev/null +++ b/main/control/DCMotorActuator.cpp @@ -0,0 +1,151 @@ +#include "control/DCMotorActuator.h" + + +#include + +#include "util/number_utils.h" + +#include "driver/ledc.h" +#include "constants/module.h" + +#include "AngleControlMessageBuilder.h" + +#define LOW_DUTY 200 +#define HIGH_DUTY 1000 +#define FWD_CHANNEL LEDC_CHANNEL_0 +#define REV_CHANNEL LEDC_CHANNEL_1 +#define DEADZONE 0.1 +#define KP 0.07 +#define KI 0.0075 +#define KD 0.065 +#define MIN_PWM_DUTY 675 +#define MAX_PWM_DUTY 1024 + +DCMotorActuator::DCMotorActuator() { + ledc_timer_config_t ledc_timer = { + .speed_mode = LEDC_LOW_SPEED_MODE, + .duty_resolution = LEDC_TIMER_10_BIT, + .timer_num = LEDC_TIMER_0, + .freq_hz = 50000, // 4kHz + .clk_cfg = LEDC_AUTO_CLK, + }; + ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); + + ledc_channel_config_t fwd_ledc_channel = { + .gpio_num = DC_MOTOR_PWM_FWD, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = FWD_CHANNEL, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_0, + .duty = 0, + .hpoint = 0, + }; + ESP_ERROR_CHECK(ledc_channel_config(&fwd_ledc_channel)); + + ledc_channel_config_t rev_ledc_channel = { + .gpio_num = DC_MOTOR_PWM_REV, + .speed_mode = LEDC_LOW_SPEED_MODE, + .channel = REV_CHANNEL, + .intr_type = LEDC_INTR_DISABLE, + .timer_sel = LEDC_TIMER_0, + .duty = 0, + .hpoint = 0, + }; + + ESP_ERROR_CHECK(ledc_channel_config(&rev_ledc_channel)); + + setup_encoder(); + + this->m_pid_task = nullptr; + this->m_target_angle = 0; + this->m_integral = 0; + this->m_last_error = 0; + + xTaskCreate(reinterpret_cast(pid_task), "pid_task", 3072, this, 1, &this->m_pid_task); +} + +DCMotorActuator::~DCMotorActuator() { + vTaskDelete(m_pid_task); +} + +volatile int32_t encoder_ticks = 0; +volatile int8_t direction = 0; + +static void IRAM_ATTR encoder_isr_handler(void* arg) { + const int a = gpio_get_level(static_cast(DC_ENCODER_A)); + const int b = gpio_get_level(static_cast(DC_ENCODER_B)); + + // Determine direction + if (a == b) { + encoder_ticks++; + direction = 1; + } else { + encoder_ticks--; + direction = -1; + } +} + +void DCMotorActuator::setup_encoder() { + gpio_config_t io_conf = {}; + io_conf.intr_type = GPIO_INTR_ANYEDGE; + io_conf.mode = GPIO_MODE_INPUT; + io_conf.pin_bit_mask = (1ULL << DC_ENCODER_A) | (1ULL << DC_ENCODER_B); + io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE; + io_conf.pull_up_en = GPIO_PULLUP_ENABLE; + gpio_config(&io_conf); + + gpio_install_isr_service(0); + gpio_isr_handler_add(static_cast(DC_ENCODER_A), encoder_isr_handler, nullptr); +} + +void DCMotorActuator::actuate(uint8_t *cmd) { + const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); + this->m_target_angle = angleControlCmd->angle(); +} + +void DCMotorActuator::pid_task(char* args) { + const auto that = reinterpret_cast(args); + + while (true) { + const double degrees = (encoder_ticks * 360.0) / (298.0 * 16); + + const double error = degrees - that->m_target_angle; + that->m_integral += error * KI; + const double detivative = (error - that->m_last_error) * KD; + that->m_last_error = error; + + double control = error * KP + that->m_integral + detivative; + if (control > 1) { + control = 1; + } else if (control < -1) { + control = -1; + } + const auto pwm = util::mapRange(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY); + + if (std::abs(control) < DEADZONE) { + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL)); + + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL)); + vTaskDelay(300 / portTICK_PERIOD_MS); + continue; + } + + if (control > 0) { + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL)); + + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, pwm)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL)); + } else { + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL)); + + ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, pwm)); + ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL)); + } + + vTaskDelay(75 / portTICK_PERIOD_MS); + } +} diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp index 5df9de2..25e6e1f 100644 --- a/main/control/Servo1Actuator.cpp +++ b/main/control/Servo1Actuator.cpp @@ -3,6 +3,9 @@ // #include "control/Servo1Actuator.h" + +#include + #include "util/number_utils.h" #include "driver/ledc.h" @@ -12,13 +15,14 @@ #define LOW_DUTY 200 #define HIGH_DUTY 1000 +#define PWM_FREQ 50 // 4khz Servo1Actuator::Servo1Actuator() { ledc_timer_config_t ledc_timer = { .speed_mode = LEDC_LOW_SPEED_MODE, .duty_resolution = LEDC_TIMER_13_BIT, .timer_num = LEDC_TIMER_0, - .freq_hz = 50, // 4kHz + .freq_hz = PWM_FREQ, .clk_cfg = LEDC_AUTO_CLK, }; ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer)); @@ -29,7 +33,7 @@ Servo1Actuator::Servo1Actuator() { .channel = LEDC_CHANNEL_0, .intr_type = LEDC_INTR_DISABLE, .timer_sel = LEDC_TIMER_0, - .duty = 600, // midpoint + .duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially .hpoint = 0, }; @@ -37,16 +41,9 @@ Servo1Actuator::Servo1Actuator() { } void Servo1Actuator::actuate(uint8_t *cmd) { - for (int i = 0; i < 512; i++) { - printf("%x ", cmd[i]); - } - printf("\n"); const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd); - std::cout << "cmd: " << angleControlCmd->angle() << std::endl; const auto newDuty = util::mapRange(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY); - std::cout << "newDuty: " << newDuty << std::endl; ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty)); ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0)); } - diff --git a/main/include/control/DCMotorActuator.h b/main/include/control/DCMotorActuator.h new file mode 100644 index 0000000..cf512d6 --- /dev/null +++ b/main/include/control/DCMotorActuator.h @@ -0,0 +1,28 @@ +// DC motor with quatrature hall encoder + +#ifndef DCMOTORACTUATOR_H +#define DCMOTORACTUATOR_H + +#include "freertos/FreeRTOS.h" +#include + + +#include "IActuator.h" + +class DCMotorActuator final : public IActuator { +public: + DCMotorActuator(); + ~DCMotorActuator() override; + void actuate(uint8_t *cmd) override; +private: + void setup_encoder(); + static void pid_task(char* args); + + int64_t m_target_angle; + TaskHandle_t m_pid_task; + + double m_integral; + double m_last_error; +}; + +#endif //SERVO1ACTUATOR_H diff --git a/main/main.cpp b/main/main.cpp index 5580aba..a408f4e 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -17,7 +17,8 @@ extern "C" [[noreturn]] void app_main(void) { ConfigManager::init_config(); - ConfigManager::set_module_type(ModuleType_SERVO_1); + ConfigManager::set_module_type(ModuleType_DC_MOTOR); + ConfigManager::set_module_id(9); LoopManager::control_loop(); }