botchain/firmware
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/firmware.git synced 2026-07-09 01:57: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

5
main/RemoteDebugging.cpp
View File

@@ -69,7 +69,6 @@ std::string RemoteDebugging::get_coredump_summary() {
// Panic reason
char reason[200] = {};
if (esp_core_dump_get_panic_reason(reason, sizeof(reason)) == ESP_OK) {
ESP_LOGI(TAG, "Panic reason: %s", reason);
out += "Panic reason: ";
out += reason;
out += "\n";
@@ -88,8 +87,6 @@ std::string RemoteDebugging::get_coredump_summary() {
summary->ex_info.exc_vaddr);
out += line;
ESP_LOGI(TAG, "%s", line);
// Backtrace
out += "Backtrace:";
for (uint32_t i = 0; i < summary->exc_bt_info.depth && i < 16; i++) {
@@ -102,8 +99,6 @@ std::string RemoteDebugging::get_coredump_summary() {
}
out += "\n";
ESP_LOGI(TAG, "Backtrace: %s", out);
free(summary);
return out;
}

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}};
}

2
main/control/Servo1Actuator.cpp
View File

@@ -86,7 +86,7 @@ void Servo1Actuator::actuate(uint8_t *cmd) {
const auto *angleControlCmd =
Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
m_target = static_cast<float>(angleControlCmd->angle());
// ESP_LOGI("Servo1", "new target: %d", angleControlCmd->angle());
ESP_LOGI("Servo1", "new target: %d", angleControlCmd->angle());
}
std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {

17
main/include/control/DCMotorActuator.h
View File

@@ -6,6 +6,7 @@
#include "IActuator.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <atomic>
class DCMotorActuator final : public IActuator {
public:
@@ -16,14 +17,16 @@ class DCMotorActuator final : public IActuator {
private:
void setup_encoder();
static void pid_task(char *args);
static void pid_task(void *args);
double m_current_angle;
int64_t m_target_angle;
TaskHandle_t m_pid_task;
volatile int32_t m_encoder_ticks{0};
double m_integral;
double m_last_error;
double m_current_angle{0.0};
std::atomic<int64_t> m_target_angle{0};
TaskHandle_t m_pid_task{nullptr};
double m_integral{0.0};
double m_last_error{0.0};
};
#endif //SERVO1ACTUATOR_H
#endif //DCMOTORACTUATOR_H

1
main/main.cpp
View File

@@ -21,7 +21,6 @@ extern "C" [[noreturn]] void app_main(void) {
auto& config_manager = ConfigManager::get_instance(); // NOLINT - here for easily adding temporary config
config_manager.set_communication_method(CommunicationMethod::Wireless);
esp_log_set_vprintf(RemoteDebugging::custom_log_write);
const auto loop_manager = std::make_unique<LoopManager>();

2
main/main_rmt_test.cpp
View File

@@ -267,7 +267,7 @@ extern "C" [[noreturn]] void app_main(void) {
// uint8_t iteration = 0;
// const char* message = "THIS IS A TEXT MESSAGE";
uint8_t num_channels = 4;
uint8_t num_channels = 3;
std::unique_ptr<DataLinkManager> obj = std::make_unique<DataLinkManager>(BOARD_ID, num_channels);
if (obj->ready() != ESP_OK){