From aee29c47e4beda182672a1643da1d31d791a32f2 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 11 Oct 2025 15:29:18 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Improves=20mode=20handling?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 esp32/include/peripherals/servo_controller.h |  3 ++-
 esp32/src/main.cpp                           | 14 ++++++--------
 2 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/esp32/include/peripherals/servo_controller.h b/esp32/include/peripherals/servo_controller.h
index 5b41353..6557283 100644
--- a/esp32/include/peripherals/servo_controller.h
+++ b/esp32/include/peripherals/servo_controller.h
@@ -78,8 +78,9 @@ class ServoController : public StatefulService<ServoSettings> {
 
     void updateActiveState() { is_active ? activate() : deactivate(); }
 
+    void setMode(SERVO_CONTROL_STATE newMode) { control_state = newMode; }
+
     void setAngles(float new_angles[12]) {
-        control_state = SERVO_CONTROL_STATE::ANGLE;
         for (int i = 0; i < 12; i++) {
             target_angles[i] = new_angles[i];
         }
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index 018a35a..1042126 100644
--- a/esp32/src/main.cpp
+++ b/esp32/src/main.cpp
@@ -39,7 +39,7 @@ WiFiService wifiService;
 APService apService;
 
 void setupServer() {
-    server.config.max_uri_handlers = 5 + WWW_ASSETS_COUNT;
+    server.config.max_uri_handlers = 10 + WWW_ASSETS_COUNT;
     server.maxUploadSize = 1000000; // 1 MB;
     server.listen(80);
     server.serveStatic("/api/config/", ESP_FS, "/config/");
@@ -87,7 +87,11 @@ void setupEventSocket() {
     // Motion events
     socket.onEvent(INPUT_EVENT, [&](JsonVariant &root, int originId) { motionService.handleInput(root, originId); });
 
-    socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) { motionService.handleMode(root, originId); });
+    socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) {
+        servoController.setMode(SERVO_CONTROL_STATE::ANGLE);
+        motionService.handleMode(root, originId);
+        motionService.isActive() ? servoController.activate() : servoController.deactivate();
+    });
 
     socket.onEvent(WALK_GAIT_EVENT,
                    [&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); });
@@ -108,11 +112,6 @@ void setupEventSocket() {
                    [&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
     socket.onEvent(EVENT_SERVO_STATE,
                    [&](JsonVariant &root, int originId) { servoController.stateUpdate(root, originId); });
-
-    socket.onEvent(EVENT_SERVO_STATE, [&](JsonVariant &root, int originId) {
-        const bool is_active = root["active"] | false;
-        is_active ? servoController.activate() : servoController.deactivate();
-    });
 }
 
 void IRAM_ATTR SpotControlLoopEntry(void *) {
@@ -129,7 +128,6 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
         peripherals.update();
         motionService.update(&peripherals);
         servoController.setAngles(motionService.getAngles());
-        motionService.isActive() ? servoController.activate() : servoController.deactivate();
         servoController.update();
 #if FT_ENABLED(USE_WS2812)
         ledService.loop();