✨ Adds skill system

esp32/include/motion.h

@@ -3,6 +3,7 @@
 
 #include <ArduinoJson.h>
 #include "esp_timer.h"
+#include <functional>
 
 #include <kinematics.h>
 #include <peripherals/gesture.h>
@@ -18,6 +19,8 @@
 
 enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 
+using SkillCompleteCallback = std::function<void()>;
+
 class MotionService {
   public:
     void begin();
@@ -30,6 +33,10 @@ class MotionService {
 
     void handleMode(JsonVariant &root, int originId);
 
+    void handleDisplacement(JsonVariant &root, int originId);
+
+    void handleSkill(JsonVariant &root, int originId);
+
     void setState(MotionState *newState);
 
     void handleGestures(const gesture_t ges);
@@ -42,6 +49,39 @@ class MotionService {
 
     inline bool isActive() { return state != nullptr; }
 
+    void resetDisplacement() { body_state.resetDisplacement(); }
+
+    void setSkillTarget(float x, float z, float yaw) { body_state.skill.set(x, z, yaw); }
+
+    void clearSkill() { body_state.skill.reset(); }
+
+    bool isSkillActive() const { return body_state.skill.active; }
+
+    bool isSkillComplete() const { return body_state.skill.isComplete(); }
+
+    const displacement_state_t &getDisplacement() const { return body_state.cumulative; }
+
+    const skill_target_t &getSkill() const { return body_state.skill; }
+
+    void getDisplacementResult(JsonVariant &root) const {
+        root["x"] = body_state.cumulative.x;
+        root["y"] = body_state.cumulative.y;
+        root["z"] = body_state.cumulative.z;
+        root["yaw"] = body_state.cumulative.yaw;
+        root["distance"] = body_state.cumulative.distance();
+        root["skill_active"] = body_state.skill.active;
+        root["skill_target_x"] = body_state.skill.target_x;
+        root["skill_target_z"] = body_state.skill.target_z;
+        root["skill_target_yaw"] = body_state.skill.target_yaw;
+        root["skill_traveled_x"] = body_state.skill.traveled_x;
+        root["skill_traveled_z"] = body_state.skill.traveled_z;
+        root["skill_rotated"] = body_state.skill.rotated;
+        root["skill_progress"] = body_state.skill.progress();
+        root["skill_complete"] = body_state.skill.isComplete();
+    }
+
+    void setSkillCompleteCallback(SkillCompleteCallback callback) { skillCompleteCallback = callback; }
+
   private:
     Kinematics kinematics;
 
@@ -63,6 +103,11 @@ class MotionService {
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
 
     int64_t lastUpdate = esp_timer_get_time();
+
+    SkillCompleteCallback skillCompleteCallback = nullptr;
+    bool skillWasComplete = false;
+
+    void checkSkillComplete();
 };
 
 #endif