✨ Adds skilmanager and spin skill

esp32/include/motion.h

@@ -11,6 +11,7 @@
 #include <motion_states/walk_state.h>
 #include <motion_states/stand_state.h>
 #include <motion_states/rest_state.h>
+#include <motion_skills/skill_manager.h>
 #include <message_types.h>
 
 #define DEFAULT_STATE false
@@ -39,6 +40,8 @@ class MotionService {
                            std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
 
         body_state.updateFeet(KinConfig::default_feet_positions);
+
+        skillManager.setWalkState(&walkState);
     }
 
     void anglesEvent(JsonVariant &root, int originId) {
@@ -109,12 +112,42 @@ class MotionService {
         const gesture_t ges = _peripherals->takeGesture();
         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;
 
+            // Check if this gesture maps to a skill
+            if (ges == gesture_t::eGestureClockwise || ges == gesture_t::eGestureAntiClockwise) {
+                skillManager.queueGestureSkill(ges);
+                return; // Let skill manager handle state transitions
+            }
+
+            // Handle basic gestures that don't require skills
+            switch (ges) {
+                case gesture_t::eGestureDown:
+                    skillManager.clearQueue(); // Clear any running skills
+                    if (state == &restState) {
+                        _servoController->deactivate();
+                        setState(nullptr);
+                    } else if (state == &standState)
+                        setState(&restState);
+                    else if (state == &walkState)
+                        setState(&standState);
+                    break;
+                case gesture_t::eGestureUp:
+                    skillManager.clearQueue(); // Clear any running skills
+
+                    if (!state) {
+                        _servoController->activate();
+                        setState(&restState);
+                    } else if (state == &restState)
+                        setState(&standState);
+                    else if (state == &standState)
+                        setState(&walkState);
+                    break;
+                    break;
+                case gesture_t::eGestureLeft:
+                case gesture_t::eGestureRight:
+                    skillManager.clearQueue(); // Clear any running skills
+                    setState(&walkState);
+                    break;
                 default: break;
             }
         }
@@ -122,10 +155,24 @@ class MotionService {
 
     bool updateMotion() {
         handleGestures();
-        if (!state) return false;
+
         unsigned long now = millis();
         float dt = (now - lastUpdate) / 1000.0f;
         lastUpdate = now;
+
+        // Update skill manager
+        skillManager.update(body_state, state, _peripherals, dt);
+
+        // If a skill is active and requires a specific state, ensure we're in that state
+        if (skillManager.hasActiveSkill()) {
+            MotionState *requiredState = skillManager.getCurrentSkillRequiredState();
+            if (requiredState && state != requiredState) {
+                setState(requiredState);
+            }
+        }
+
+        if (!state) return false;
+
         state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX());
         state->step(body_state, dt);
         kinematics.calculate_inverse_kinematics(body_state, new_angles);
@@ -162,6 +209,8 @@ class MotionService {
     StandState standState;
     WalkState walkState;
 
+    SkillManager skillManager;
+
     body_state_t body_state;
 
     float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

esp32/include/motion_skills/skill.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <kinematics.h>
+#include <message_types.h>
+#include <peripherals/peripherals.h>
+#include <motion_states/state.h>
+
+class Skill {
+  public:
+    virtual ~Skill() = default;
+
+    virtual const char* getName() const = 0;
+
+    virtual void begin(body_state_t& body_state, Peripherals* peripherals) {}
+
+    virtual void execute(body_state_t& body_state, MotionState* currentState, Peripherals* peripherals, float dt) = 0;
+
+    virtual bool isComplete() const = 0;
+
+    virtual void reset() = 0;
+
+    virtual MotionState* getRequiredState() = 0;
+
+  protected:
+    bool _isActive = false;
+    bool _isComplete = false;
+};

esp32/include/motion_skills/skill_manager.h

@@ -0,0 +1,96 @@
+#pragma once
+
+#include <motion_skills/skill.h>
+#include <motion_skills/spin_skill.h>
+#include <motion_states/walk_state.h>
+#include <peripherals/gesture.h>
+#include <esp_log.h>
+#include <queue>
+#include <memory>
+
+class SkillManager {
+  private:
+    std::queue<std::unique_ptr<Skill>> _skillQueue;
+    std::unique_ptr<Skill> _currentSkill;
+    WalkState* _walkState = nullptr;
+
+  public:
+    SkillManager() = default;
+
+    void setWalkState(WalkState* walkState) { _walkState = walkState; }
+
+    void queueSkill(std::unique_ptr<Skill> skill) {
+        _skillQueue.push(std::move(skill));
+        ESP_LOGI("SkillManager", "Queued skill. Queue size: %d", _skillQueue.size());
+    }
+
+    void queueGestureSkill(gesture_t gesture) {
+        std::unique_ptr<Skill> skill = nullptr;
+
+        switch (gesture) {
+            case gesture_t::eGestureClockwise:
+                skill = std::make_unique<SpinAroundSkill>(true);
+                static_cast<SpinAroundSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureAntiClockwise:
+                skill = std::make_unique<SpinAroundSkill>(false);
+                static_cast<SpinAroundSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            default: return; // No skill mapped to this gesture
+        }
+
+        if (skill) {
+            ESP_LOGI("SkillManager", "Mapping gesture %d to skill: %s", gesture, skill->getName());
+            queueSkill(std::move(skill));
+        }
+    }
+
+    void update(body_state_t& body_state, MotionState* currentState, Peripherals* peripherals, float dt) {
+        // Check if current skill is complete
+        if (_currentSkill && _currentSkill->isComplete()) {
+            ESP_LOGI("SkillManager", "Skill '%s' completed", _currentSkill->getName());
+            _currentSkill.reset();
+        }
+
+        // Start next skill if no current skill and queue has skills
+        if (!_currentSkill && !_skillQueue.empty()) {
+            _currentSkill = std::move(_skillQueue.front());
+            _skillQueue.pop();
+            _currentSkill->begin(body_state, peripherals);
+            ESP_LOGI("SkillManager", "Started skill: %s", _currentSkill->getName());
+        }
+
+        // Execute current skill
+        if (_currentSkill && !_currentSkill->isComplete()) {
+            _currentSkill->execute(body_state, currentState, peripherals, dt);
+        }
+    }
+
+    bool hasActiveSkill() const { return _currentSkill && !_currentSkill->isComplete(); }
+
+    bool hasQueuedSkills() const { return !_skillQueue.empty(); }
+
+    void clearQueue() {
+        while (!_skillQueue.empty()) {
+            _skillQueue.pop();
+        }
+        if (_currentSkill) {
+            _currentSkill->reset();
+            _currentSkill.reset();
+        }
+        ESP_LOGI("SkillManager", "Cleared all skills");
+    }
+
+    const char* getCurrentSkillName() const { return _currentSkill ? _currentSkill->getName() : "None"; }
+
+    MotionState* getCurrentSkillRequiredState() const {
+        return _currentSkill ? _currentSkill->getRequiredState() : nullptr;
+    }
+
+    void logStatus() const {
+        ESP_LOGI("SkillManager", "Status: active=%s, queued=%d, current=%s", hasActiveSkill() ? "yes" : "no",
+                 _skillQueue.size(), getCurrentSkillName());
+    }
+};

esp32/include/motion_skills/spin_skill.h

@@ -0,0 +1,120 @@
+#pragma once
+
+#include <motion_skills/skill.h>
+#include <utils/math_utils.h>
+#include <esp_log.h>
+
+// Forward declaration to avoid circular dependency
+class WalkState;
+
+class SpinAroundSkill : public Skill {
+  private:
+    float _startHeading = -1.0f;
+    float _targetRotation = 0.0f;
+    float _currentRotation = 0.0f;
+    float _lastHeading = -1.0f;
+    unsigned long _startTime = 0;
+    bool _clockwise = true;
+    WalkState* _walkState = nullptr;
+    static constexpr float HEADING_TOLERANCE = 15.0f; // degrees
+    static constexpr float SPIN_SPEED = 0.75f;
+    static constexpr float MIN_HEADING_CHANGE = 5.0f;  // minimum change to register movement
+    static constexpr unsigned long TIMEOUT_MS = 30000; // 30 second timeout
+
+  public:
+    SpinAroundSkill(bool clockwise = true) : _clockwise(clockwise) {}
+
+    const char* getName() const override { return _clockwise ? "Spin Clockwise" : "Spin Counter-Clockwise"; }
+
+    void begin(body_state_t& body_state, Peripherals* peripherals) override {
+        _isActive = true;
+        _isComplete = false;
+        _startHeading = peripherals->getHeading();
+        _lastHeading = _startHeading;
+        _currentRotation = 0.0f;
+        _targetRotation = 360.0f;
+        _startTime = millis();
+        ESP_LOGI("SpinSkill", "Starting %s from heading %.1f", getName(), _startHeading);
+    }
+
+    void execute(body_state_t& body_state, MotionState* currentState, Peripherals* peripherals, float dt) override {
+        if (!_isActive || _isComplete) return;
+
+        float currentHeading = peripherals->getHeading();
+
+        // Check for valid heading data
+        if (currentHeading < 0.0f || _startHeading < 0.0f) {
+            ESP_LOGW("SpinSkill", "Invalid heading - start: %.1f, current: %.1f, continuing rotation", _startHeading,
+                     currentHeading);
+        } else if (_lastHeading >= 0.0f) {
+            // Calculate the change in heading since last update
+            float headingDelta = currentHeading - _lastHeading;
+
+            // Handle wrap-around (crossing 0°/360° boundary)
+            if (headingDelta > 180.0f) {
+                headingDelta -= 360.0f;
+            } else if (headingDelta < -180.0f) {
+                headingDelta += 360.0f;
+            }
+
+            // Accumulate rotation in the correct direction
+            if (_clockwise && headingDelta < 0.0f) {
+                // Clockwise rotation shows as negative heading change
+                _currentRotation += -headingDelta;
+            } else if (!_clockwise && headingDelta > 0.0f) {
+                // Counter-clockwise rotation shows as positive heading change
+                _currentRotation += headingDelta;
+            }
+
+            // Prevent accumulation of small noise/jitter
+            if (std::abs(headingDelta) < MIN_HEADING_CHANGE) {
+                // Don't accumulate very small changes
+            }
+
+            ESP_LOGI("SpinSkill", "%s: %.1f°->%.1f° (Δ%.1f°) total=%.1f°/%.1f°", getName(), _lastHeading,
+                     currentHeading, headingDelta, _currentRotation, _targetRotation);
+        }
+
+        _lastHeading = currentHeading;
+
+        // Check for timeout
+        if (millis() - _startTime > TIMEOUT_MS) {
+            _isComplete = true;
+            ESP_LOGW("SpinSkill", "Timeout %s - rotated %.1f/%.1f degrees", getName(), _currentRotation,
+                     _targetRotation);
+            return;
+        }
+
+        // Check if we've completed a full rotation
+        if (_currentRotation >= (_targetRotation - HEADING_TOLERANCE)) {
+            _isComplete = true;
+            ESP_LOGI("SpinSkill", "Completed %s - rotated %.1f degrees", getName(), _currentRotation);
+            return;
+        }
+
+        // Apply spin command to current state
+        if (currentState) {
+            CommandMsg spinCommand = {0};
+            spinCommand.h = 0.75f;
+            spinCommand.rx = _clockwise ? SPIN_SPEED : -SPIN_SPEED;
+            spinCommand.s1 = 0.75f;
+            spinCommand.s = 0.7f; // Medium speed
+            currentState->handleCommand(spinCommand);
+        }
+    }
+
+    bool isComplete() const override { return _isComplete; }
+
+    void reset() override {
+        _isActive = false;
+        _isComplete = false;
+        _startHeading = -1.0f;
+        _lastHeading = -1.0f;
+        _currentRotation = 0.0f;
+        _startTime = 0;
+    }
+
+    MotionState* getRequiredState() override { return _walkState; }
+
+    void setWalkState(WalkState* walkState) { _walkState = walkState; }
+};

esp32/include/peripherals/peripherals.h

@@ -209,6 +209,14 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
     float leftDistance() { return _left_distance; }
     float rightDistance() { return _right_distance; }
 
+    float getHeading() {
+#if FT_ENABLED(USE_HMC5883)
+        return _mag.getHeading();
+#else
+        return 0.0f;
+#endif
+    }
+
     StatefulHttpEndpoint<PeripheralsConfiguration> endpoint;
 
     void emitIMU() {