♻️ 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) {
-        JsonArray array = root.as<JsonArray>();
-        for (int i = 0; i < 12; i++) {
-            angles[i] = array[i];
-        }
-        syncAngles(String(originId));
-    }
+    void handleInput(JsonVariant &root, int originId);
 
-    void setState(MotionState *newState) {
-        if (state) {
-            state->end();
-        }
-        state = newState;
-        if (state) {
-            _servoController->activate();
-            state->begin();
-        }
-    }
+    void handleWalkGait(JsonVariant &root, int originId);
 
-    void handleInput(JsonVariant &root, int originId) {
-        command.fromJson(root);
-        if (state) state->handleCommand(command);
-    }
+    void handleMode(JsonVariant &root, int originId);
 
-    void handleWalkGait(JsonVariant &root, int originId) {
-        ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
+    void setState(MotionState *newState);
 
-        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 handleGestures(const gesture_t ges);
 
-    void 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);
-                _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());
-    }
+    bool update(Peripherals *peripherals);
 
-    void emitAngles(const String &originId = "", bool sync = false) {
-        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;
-    }
+    bool update_angles(float new_angles[12], float angles[12]);
 
     float *getAngles() { return angles; }
 
     inline bool isActive() { return state != nullptr; }
 
   private:
-    ServoController *_servoController;
     Kinematics kinematics;
 
     CommandMsg command = {0, 0, 0, 0, 0, 0, 0};