mirror of
https://github.com/BotChain-Robots/firmware.git
synced 2026-07-08 17:47:21 +02:00
95 lines
3.0 KiB
C++
95 lines
3.0 KiB
C++
//
|
|
// Created by Johnathon Slightham on 2025-07-15.
|
|
//
|
|
|
|
#include "control/Servo1Actuator.h"
|
|
#include "AngleControlMessageBuilder.h"
|
|
#include "SensorMessageBuilder.h"
|
|
#include "constants/module.h"
|
|
#include "driver/ledc.h"
|
|
#include "flatbuffers_generated/SensorMessage_generated.h"
|
|
#include "util/number_utils.h"
|
|
#include "esp_log.h"
|
|
#include <cmath>
|
|
|
|
#define LOW_DUTY 200
|
|
#define HIGH_DUTY 1000
|
|
#define PWM_FREQ 50 // 50 Hz for servo
|
|
|
|
// Task period in milliseconds
|
|
#define TASK_PERIOD_MS 20
|
|
// Max degrees to move per task tick
|
|
#define MAX_DEG_PER_TICK (SERVO_MAX_SPEED_DEG_PER_SEC * (TASK_PERIOD_MS / 1000.0f))
|
|
|
|
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 = PWM_FREQ,
|
|
.clk_cfg = LEDC_AUTO_CLK,
|
|
};
|
|
ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
|
|
|
|
ledc_channel_config_t ledc_channel = {
|
|
.gpio_num = SERVO_GPIO,
|
|
.speed_mode = LEDC_LOW_SPEED_MODE,
|
|
.channel = LEDC_CHANNEL_0,
|
|
.intr_type = LEDC_INTR_DISABLE,
|
|
.timer_sel = LEDC_TIMER_0,
|
|
.duty = (HIGH_DUTY + LOW_DUTY) / 2, // move motor to midpoint initially
|
|
.hpoint = 0,
|
|
};
|
|
ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
|
|
|
|
xTaskCreate(servo_task, "servo_task", 4096, this, 1, &m_servo_task);
|
|
}
|
|
|
|
Servo1Actuator::~Servo1Actuator() {
|
|
m_running = false;
|
|
if (m_servo_task != nullptr) {
|
|
vTaskDelete(m_servo_task);
|
|
}
|
|
}
|
|
|
|
void Servo1Actuator::write_pwm(float angle) {
|
|
const auto duty = util::mapRange<float>(angle, 0.0f, 180.0f, LOW_DUTY, HIGH_DUTY);
|
|
ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, static_cast<uint32_t>(duty)));
|
|
ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
|
|
}
|
|
|
|
void Servo1Actuator::servo_task(void *args) {
|
|
auto *that = static_cast<Servo1Actuator *>(args);
|
|
|
|
while (that->m_running) {
|
|
const float target = that->m_target.load();
|
|
const float current = that->m_current;
|
|
const float diff = target - current;
|
|
|
|
if (std::abs(diff) > 0.001f) {
|
|
float step = diff;
|
|
if (std::abs(step) > MAX_DEG_PER_TICK) {
|
|
step = (diff > 0.0f ? 1.0f : -1.0f) * MAX_DEG_PER_TICK;
|
|
}
|
|
that->m_current += step;
|
|
// ESP_LOGI("Servo1", "current=%.2f target=%.2f", that->m_current, target);
|
|
write_pwm(that->m_current);
|
|
}
|
|
|
|
vTaskDelay(pdMS_TO_TICKS(TASK_PERIOD_MS));
|
|
}
|
|
|
|
vTaskDelete(nullptr);
|
|
}
|
|
|
|
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());
|
|
}
|
|
|
|
std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
|
|
return {Flatbuffers::target_angle{static_cast<int16_t>(m_current)}};
|
|
}
|