From 870e87b428c23cf12386005fb323129aa27c1484 Mon Sep 17 00:00:00 2001
From: Johnathon Slightham <johnathon.slightham@uwaterloo.ca>
Date: Mon, 2 Mar 2026 22:28:37 -0500
Subject: [PATCH] Js/servo max speed

---
 main/control/Servo1Actuator.cpp       | 66 +++++++++++++++++++++------
 main/include/control/Servo1Actuator.h | 14 +++++-
 2 files changed, 64 insertions(+), 16 deletions(-)

diff --git a/main/control/Servo1Actuator.cpp b/main/control/Servo1Actuator.cpp
index 70cd500..a4fde71 100644
--- a/main/control/Servo1Actuator.cpp
+++ b/main/control/Servo1Actuator.cpp
@@ -10,10 +10,16 @@
 #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 // 4khz
+#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 = {
@@ -34,25 +40,55 @@ Servo1Actuator::Servo1Actuator() {
         .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);
 }
 
-void Servo1Actuator::actuate(uint8_t *cmd) {
-    // todo: Do we want to verify flatbuffers here?
-    // 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);
-    const auto newDuty =
-        util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
+Servo1Actuator::~Servo1Actuator() {
+    m_running = false;
+    if (m_servo_task != nullptr) {
+        vTaskDelete(m_servo_task);
+    }
+}
 
-    m_target = angleControlCmd->angle();
-    ESP_LOGI("TMP", "actuating to %d", angleControlCmd->angle());
-
-    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
+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));
 }
 
-std::vector<Flatbuffers::sensor_value> Servo1Actuator::get_sensor_data() {
-    return {Flatbuffers::target_angle{(int16_t)m_target}};
+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)}};
 }
diff --git a/main/include/control/Servo1Actuator.h b/main/include/control/Servo1Actuator.h
index daa5ebe..f480c10 100644
--- a/main/include/control/Servo1Actuator.h
+++ b/main/include/control/Servo1Actuator.h
@@ -9,16 +9,28 @@
 
 #include "IActuator.h"
 #include "ISensor.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include <atomic>
 #include <cstdint>
 
+#define SERVO_MAX_SPEED_DEG_PER_SEC 180.0f
+
 class Servo1Actuator final : public IActuator {
   public:
     Servo1Actuator();
+    ~Servo1Actuator() override;
     void actuate(std::uint8_t *cmd) override;
     std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
 
   private:
-    uint16_t m_target = 90;
+    static void servo_task(void *args);
+    static void write_pwm(float angle);
+
+    std::atomic<float> m_target{90.0f};
+    float m_current{90.0f};
+    std::atomic<bool> m_running{true};
+    TaskHandle_t m_servo_task{nullptr};
 };
 
 #endif //SERVO1ACTUATOR_H