botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-08 17:47:21 +02:00
Code Issues Packages Projects Releases Wiki Activity

Fix RIP bugs, add in UART

Browse Source
This commit is contained in:
Johnathon Slightham
2026-03-31 14:26:37 -04:00
committed by Johnathon Slightham
parent d4602012f1
commit 548e8db484
26 changed files with 704 additions and 434 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
main/control/DCMotorActuator.cpp
View File

@@ -6,20 +6,23 @@
#include "control/DCMotorActuator.h"
#include "driver/ledc.h"
#include "esp_attr.h"
#include "esp_intr_alloc.h"
#include "flatbuffers_generated/SensorMessage_generated.h"
#include "util/number_utils.h"
#include "esp_log.h"
#define TAG "DCMotor"
#define LOW_DUTY 200
#define HIGH_DUTY 1000
#define FWD_CHANNEL LEDC_CHANNEL_1
#define REV_CHANNEL LEDC_CHANNEL_0
#define DEADZONE 0.05
#define KP 0.01
#define KP 0.05
#define KI 0
#define KD 0.020
#define MIN_PWM_DUTY 675
#define MAX_PWM_DUTY 1024
#define MAX_PWM_DUTY 1023
#define TICKS_PER_ROTATION 18.0
#define GEAR_RATIO 298
@@ -58,14 +61,9 @@ DCMotorActuator::DCMotorActuator() {
setup_encoder();
this->m_pid_task = nullptr;
this->m_target_angle = 0;
this->m_integral = 0;
this->m_last_error = 0;
// Pin the PID task to Core 1 so it doesn't compete with the RMT driver,
// which runs its ISR and internal tasks on Core 0.
xTaskCreatePinnedToCore(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 3072, this, 1,
xTaskCreatePinnedToCore(reinterpret_cast<TaskFunction_t>(pid_task), "pid_task", 4096, this, 1,
&this->m_pid_task, 1);
}
@@ -73,20 +71,16 @@ 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) {
auto *ticks = static_cast<volatile int32_t *>(arg);
const int a = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_A));
const int b = gpio_get_level(static_cast<gpio_num_t>(DC_ENCODER_B));
// Determine direction
if (a == b) {
encoder_ticks++;
direction = 1;
(*ticks)++;
} else {
encoder_ticks--;
direction = -1;
(*ticks)--;
}
}
@@ -99,8 +93,12 @@ void DCMotorActuator::setup_encoder() {
io_conf.pull_up_en = GPIO_PULLUP_ENABLE;
gpio_config(&io_conf);
gpio_install_isr_service(0);
gpio_isr_handler_add(static_cast<gpio_num_t>(DC_ENCODER_A), encoder_isr_handler, nullptr);
esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {
ESP_ERROR_CHECK(err);
}
gpio_isr_handler_add(static_cast<gpio_num_t>(DC_ENCODER_A), encoder_isr_handler,
reinterpret_cast<void *>(const_cast<int32_t *>(&m_encoder_ticks)));
}
void DCMotorActuator::actuate(uint8_t *cmd) {
@@ -108,51 +106,67 @@ void DCMotorActuator::actuate(uint8_t *cmd) {
// Will introduce latency, and means that we need to also send the size to the actuate function.
const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
this->m_target_angle = angleControlCmd->angle();
this->m_target_angle.store(angleControlCmd->angle(), std::memory_order_relaxed);
}
void DCMotorActuator::pid_task(char *args) {
void DCMotorActuator::pid_task(void *args) {
const auto that = reinterpret_cast<DCMotorActuator *>(args);
while (true) {
that->m_current_angle = (encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
that->m_current_angle = (that->m_encoder_ticks * 360.0) / (GEAR_RATIO * TICKS_PER_ROTATION);
const double error = that->m_current_angle - that->m_target_angle;
const int64_t target = that->m_target_angle.load(std::memory_order_relaxed);
const double error = that->m_current_angle - target;
that->m_integral += error * KI;
const double detivative = (error - that->m_last_error) * KD;
const double derivative = (error - that->m_last_error) * KD;
that->m_last_error = error;
double control = error * KP + that->m_integral + detivative;
double control = error * KP + that->m_integral + derivative;
if (control > 1) {
control = 1;
} else if (control < -1) {
control = -1;
}
const auto pwm =
util::mapRange<double>(std::abs(control), 0, 1, MIN_PWM_DUTY, MAX_PWM_DUTY);
const uint32_t pwm = static_cast<uint32_t>(
util::mapRange<double>(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_err_t err;
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
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;
}
esp_err_t err;
if (control > 0) {
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));
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, pwm);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
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));
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, 0);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
} else {
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));
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL, pwm);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, REV_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
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));
err = ledc_set_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL, 0);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_set_duty failed: %s", esp_err_to_name(err)); }
err = ledc_update_duty(LEDC_LOW_SPEED_MODE, FWD_CHANNEL);
if (err != ESP_OK) { ESP_LOGE(TAG, "ledc_update_duty failed: %s", esp_err_to_name(err)); }
}
vTaskDelay(75 / portTICK_PERIOD_MS);
@@ -160,7 +174,6 @@ void DCMotorActuator::pid_task(char *args) {
}
std::vector<Flatbuffers::sensor_value> DCMotorActuator::get_sensor_data() {
// todo: this really needs to return a int32, should also return two sensor data items, one for target one for current
return {Flatbuffers::target_angle{(int16_t)m_current_angle},
Flatbuffers::current_angle{(int16_t)m_current_angle}};
}