♻️ Moves motion implementation to source file

esp32/include/motion.h

@@ -1,9 +1,11 @@
 #ifndef MotionService_h
 #define MotionService_h
 
-#include <event_socket.h>
+#include <ArduinoJson.h>
+
 #include <kinematics.h>
-#include <peripherals/servo_controller.h>
+#include <peripherals/gesture.h>
+#include <peripherals/peripherals.h>
 #include <utils/timing.h>
 #include <utils/math_utils.h>
 
@@ -17,121 +19,29 @@ enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 
 class MotionService {
   public:
-    MotionService() {}
+    void begin();
 
-    void begin() { body_state.updateFeet(KinConfig::default_feet_positions); }
+    void anglesEvent(JsonVariant &root, int originId);
 
-    void anglesEvent(JsonVariant &root, int originId) {
+    void handleInput(JsonVariant &root, int originId);
-        JsonArray array = root.as<JsonArray>();
-        for (int i = 0; i < 12; i++) {
-            angles[i] = array[i];
-        }
-        syncAngles(String(originId));
-    }
 
-    void setState(MotionState *newState) {
+    void handleWalkGait(JsonVariant &root, int originId);
-        if (state) {
-            state->end();
-        }
-        state = newState;
-        if (state) {
-            _servoController->activate();
-            state->begin();
-        }
-    }
 
-    void handleInput(JsonVariant &root, int originId) {
+    void handleMode(JsonVariant &root, int originId);
-        command.fromJson(root);
-        if (state) state->handleCommand(command);
-    }
 
-    void handleWalkGait(JsonVariant &root, int originId) {
+    void setState(MotionState *newState);
-        ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
 
-        WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
+    void handleGestures(const gesture_t ges);
-        if (walkGait == WALK_GAIT::TROT)
-            walkState.set_mode_trot();
-        else
-            walkState.set_mode_crawl();
-    }
 
-    void handleMode(JsonVariant &root, int originId) {
+    bool update(Peripherals *peripherals);
-        MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
-        ESP_LOGV("MotionService", "Mode %d", mode);
-        switch (mode) {
-            case MOTION_STATE::REST: setState(&restState); break;
-            case MOTION_STATE::STAND: setState(&standState); break;
-            case MOTION_STATE::WALK: setState(&walkState); break;
-            case MOTION_STATE::DEACTIVATED:
-                setState(nullptr);
-                _servoController->deactivate();
-                break;
-            default: setState(nullptr); break;
-        }
-        JsonDocument doc;
-        doc.set(static_cast<int>(mode));
-        JsonVariant data = doc.as<JsonVariant>();
-        // socket.emit(MODE_EVENT, data, String(originId).c_str());
-    }
 
-    void emitAngles(const String &originId = "", bool sync = false) {
+    bool update_angles(float new_angles[12], float angles[12]);
-        JsonDocument doc;
-        auto arr = doc.to<JsonArray>();
-        for (int i = 0; i < 12; i++) arr.add(angles[i]);
-        JsonVariant data = doc.as<JsonVariant>();
-        // socket.emit(ANGLES_EVENT, data, originId.c_str());
-    }
-
-    void syncAngles(const String &originId = "", bool sync = false) {
-        emitAngles(originId, sync);
-        _servoController->setAngles(angles);
-    }
-
-    void handleGestures(const gesture_t ges) {
-        if (ges != gesture_t::eGestureNone) {
-            ESP_LOGI("Motion", "Gesture: %d", ges);
-            switch (ges) {
-                case gesture_t::eGestureDown: setState(&restState); break;
-                case gesture_t::eGestureUp: setState(&standState); break;
-                case gesture_t::eGestureLeft:
-                case gesture_t::eGestureRight: setState(&walkState); break;
-
-                default: break;
-            }
-        }
-    }
-
-    bool update(Peripherals *peripherals) {
-        handleGestures(peripherals->takeGesture());
-        if (!state) return false;
-        unsigned long now = millis();
-        float dt = (now - lastUpdate) / 1000.0f;
-        lastUpdate = now;
-        state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
-        state->step(body_state, dt);
-        kinematics.calculate_inverse_kinematics(body_state, new_angles);
-
-        return update_angles(new_angles, angles);
-    }
-
-    bool update_angles(float new_angles[12], float angles[12]) {
-        bool updated = false;
-        for (int i = 0; i < 12; i++) {
-            const float new_angle = new_angles[i] * dir[i];
-            if (!isEqual(new_angle, angles[i], 0.1)) {
-                angles[i] = new_angle;
-                updated = true;
-            }
-        }
-        return updated;
-    }
 
     float *getAngles() { return angles; }
 
     inline bool isActive() { return state != nullptr; }
 
   private:
-    ServoController *_servoController;
     Kinematics kinematics;
 
     CommandMsg command = {0, 0, 0, 0, 0, 0, 0};

esp32/src/main.cpp

@@ -85,9 +85,6 @@ void setupEventSocket() {
                    [&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); });
 
     socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId); });
-
-    socket.onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, motionService, std::placeholders::_1,
-                                               std::placeholders::_2));
 }
 
 void IRAM_ATTR SpotControlLoopEntry(void *) {
@@ -115,6 +112,7 @@ void IRAM_ATTR SpotControlLoopEntry(void *) {
 
 void IRAM_ATTR serviceLoopEntry(void *) {
     ESP_LOGI("main", "Service control task starting");
+
     wifiService.begin();
     MDNS.begin(APP_NAME);
     MDNS.setInstanceName(APP_NAME);

esp32/src/motion.cpp

@@ -0,0 +1,86 @@
+#include <motion.h>
+
+void MotionService::begin() { body_state.updateFeet(KinConfig::default_feet_positions); }
+
+void MotionService::anglesEvent(JsonVariant &root, int originId) {
+    JsonArray array = root.as<JsonArray>();
+    for (int i = 0; i < 12; i++) {
+        angles[i] = array[i];
+    }
+}
+
+void MotionService::setState(MotionState *newState) {
+    if (state) {
+        state->end();
+    }
+    state = newState;
+    if (state) {
+        state->begin();
+    }
+}
+
+void MotionService::handleInput(JsonVariant &root, int originId) {
+    command.fromJson(root);
+    if (state) state->handleCommand(command);
+}
+
+void MotionService::handleWalkGait(JsonVariant &root, int originId) {
+    ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
+
+    WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
+    if (walkGait == WALK_GAIT::TROT)
+        walkState.set_mode_trot();
+    else
+        walkState.set_mode_crawl();
+}
+
+void MotionService::handleMode(JsonVariant &root, int originId) {
+    MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
+    ESP_LOGV("MotionService", "Mode %d", mode);
+    switch (mode) {
+        case MOTION_STATE::REST: setState(&restState); break;
+        case MOTION_STATE::STAND: setState(&standState); break;
+        case MOTION_STATE::WALK: setState(&walkState); break;
+        case MOTION_STATE::DEACTIVATED: setState(nullptr); break;
+        default: setState(nullptr); break;
+    }
+}
+
+void MotionService::handleGestures(const gesture_t ges) {
+    if (ges != gesture_t::eGestureNone) {
+        ESP_LOGI("Motion", "Gesture: %d", ges);
+        switch (ges) {
+            case gesture_t::eGestureDown: setState(&restState); break;
+            case gesture_t::eGestureUp: setState(&standState); break;
+            case gesture_t::eGestureLeft:
+            case gesture_t::eGestureRight: setState(&walkState); break;
+
+            default: break;
+        }
+    }
+}
+
+bool MotionService::update(Peripherals *peripherals) {
+    handleGestures(peripherals->takeGesture());
+    if (!state) return false;
+    unsigned long now = millis();
+    float dt = (now - lastUpdate) / 1000.0f;
+    lastUpdate = now;
+    state->updateImuOffsets(peripherals->angleY(), peripherals->angleX());
+    state->step(body_state, dt);
+    kinematics.calculate_inverse_kinematics(body_state, new_angles);
+
+    return update_angles(new_angles, angles);
+}
+
+bool MotionService::update_angles(float new_angles[12], float angles[12]) {
+    bool updated = false;
+    for (int i = 0; i < 12; i++) {
+        const float new_angle = new_angles[i] * dir[i];
+        if (!isEqual(new_angle, angles[i], 0.1)) {
+            angles[i] = new_angle;
+            updated = true;
+        }
+    }
+    return updated;
+}