🎨 Simplifies spin skill

esp32/include/kinematics.h

@@ -184,13 +184,13 @@ class Kinematics {
     }
 
     inline void legIK(float x, float y, float z, float out[3]) {
-        float F = sqrt(max(0.0f, x * x + y * y - coxa * coxa));
+        float F = sqrt(std::max(0.0f, x * x + y * y - coxa * coxa));
         float G = F - coxa_offset;
         float H = sqrt(G * G + z * z);
 
         float theta1 = -atan2f(y, x) - atan2f(F, -coxa);
         float D = (H * H - femur * femur - tibia * tibia) / (2 * femur * tibia);
-        float theta3 = acosf(max(-1.0f, min(1.0f, D)));
+        float theta3 = acosf(std::max(-1.0f, std::min(1.0f, D)));
         float theta2 = atan2f(z, G) - atan2f(tibia * sinf(theta3), femur + tibia * cosf(theta3));
         out[0] = RAD_TO_DEG_F(theta1);
         out[1] = RAD_TO_DEG_F(theta2);

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,23 +112,49 @@ 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;
 
-                default: break;
+            if (ges == gesture_t::eGestureUp) {
+                skillManager.clearQueue();
+                if (!state) {
+                    _servoController->activate();
+                    setState(&restState);
+                } else if (state == &restState)
+                    setState(&standState);
+                else if (state == &standState)
+                    setState(&walkState);
+            } else if (ges == gesture_t::eGestureDown) {
+                skillManager.clearQueue();
+                if (state == &restState) {
+                    _servoController->deactivate();
+                    setState(nullptr);
+                } else if (state == &standState)
+                    setState(&restState);
+                else if (state == &walkState)
+                    setState(&standState);
+            } else {
+                skillManager.queueGestureSkill(ges);
             }
         }
     }
 
     bool updateMotion() {
         handleGestures();
-        if (!state) return false;
+
         unsigned long now = millis();
         float dt = (now - lastUpdate) / 1000.0f;
         lastUpdate = now;
+
+        skillManager.update(body_state, state, _peripherals, dt);
+
+        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 +191,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,123 @@
+#pragma once
+
+#include <motion_skills/skill.h>
+#include <motion_skills/spin_skill.h>
+#include <motion_skills/walk_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::eGestureLeft:
+                // Walk 1m left (90 degrees heading)
+                skill = std::make_unique<WalkSkill>(1.0f, 90.0f, 0.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureRight:
+                // Walk 1m right (-90 degrees heading)
+                skill = std::make_unique<WalkSkill>(1.0f, -90.0f, 0.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureUp:
+                // Walk 1m forward (0 degrees heading)
+                skill = std::make_unique<WalkSkill>(1.0f, 0.0f, 0.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureDown:
+                // Walk 1m backward (180 degrees heading)
+                skill = std::make_unique<WalkSkill>(-1.0f, 0.0f, 0.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureClockwise:
+                // Rotate 90 degrees clockwise
+                skill = std::make_unique<WalkSkill>(0.0f, 0.0f, 90.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
+                break;
+
+            case gesture_t::eGestureAntiClockwise:
+                // Rotate 90 degrees counter-clockwise
+                skill = std::make_unique<WalkSkill>(0.0f, 0.0f, -90.0f);
+                static_cast<WalkSkill*>(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,13 @@
+#pragma once
+
+#include <motion_skills/walk_skill.h>
+
+class SpinAroundSkill : public WalkSkill {
+  private:
+    bool _clockwise = true;
+
+  public:
+    SpinAroundSkill(bool clockwise = true) : WalkSkill(0.0f, 0.0f, clockwise ? 90.0f : -90.0f), _clockwise(clockwise) {}
+
+    const char* getName() const override { return _clockwise ? "Spin Clockwise" : "Spin Counter-Clockwise"; }
+};

esp32/include/motion_skills/walk_skill.h

@@ -0,0 +1,155 @@
+#pragma once
+
+#include <motion_skills/skill.h>
+#include <utils/math_utils.h>
+#include <esp_log.h>
+
+class WalkState;
+
+class WalkSkill : public Skill {
+  private:
+    float _targetDistance = 0.0f;
+    float _targetHeading = 0.0f;
+    float _targetRotation = 0.0f;
+
+    float _startHeading = 0.0f;
+    float _currentDistance = 0.0f;
+    float _currentRotation = 0.0f;
+
+    float _lastPhaseTime = 0.0f;
+    int _stepCount = 0;
+    float _estimatedStepLength = 0.0f;
+
+    unsigned long _startTime = 0;
+    WalkState* _walkState = nullptr;
+
+    static constexpr float DISTANCE_TOLERANCE = 0.05f;
+    static constexpr float HEADING_TOLERANCE = 10.0f;
+    static constexpr float ROTATION_TOLERANCE = 10.0f;
+    static constexpr float WALK_SPEED = 0.8f;
+    static constexpr unsigned long TIMEOUT_MS = 10000;
+
+  public:
+    WalkSkill(float distance = 0.0f, float heading = 0.0f, float rotation = 0.0f)
+        : _targetDistance(distance), _targetHeading(heading), _targetRotation(rotation) {}
+
+    const char* getName() const override { return "Walk"; }
+
+    void begin(body_state_t& body_state, Peripherals* peripherals) override {
+        _isActive = true;
+        _isComplete = false;
+        _startHeading = peripherals->getHeading();
+        _currentDistance = 0.0f;
+        _currentRotation = 0.0f;
+        _lastPhaseTime = 0.0f;
+        _stepCount = 0;
+        _estimatedStepLength = 0.0f;
+        _startTime = millis();
+
+        ESP_LOGI("WalkSkill", "Starting walk: %.2fm forward, %.1f° heading, %.1f° rotation", _targetDistance,
+                 _targetHeading, _targetRotation);
+    }
+
+    void execute(body_state_t& body_state, MotionState* currentState, Peripherals* peripherals, float dt) override {
+        if (!_isActive || _isComplete) return;
+
+        if (millis() - _startTime > TIMEOUT_MS) {
+            _isComplete = true;
+            ESP_LOGW("WalkSkill", "Timeout - walk incomplete");
+            return;
+        }
+
+        float currentPhaseTime = fmod((millis() - _startTime) * 0.001f * WALK_SPEED * 2.0f, 1.0f);
+
+        if (currentPhaseTime < _lastPhaseTime) {
+            _stepCount++;
+
+            _estimatedStepLength = WALK_SPEED * 0.08f;
+
+            _currentDistance = _stepCount * _estimatedStepLength;
+
+            ESP_LOGI("WalkSkill", "Step %d completed, estimated length: %.3fm, total distance: %.3fm", _stepCount,
+                     _estimatedStepLength, _currentDistance);
+        }
+        _lastPhaseTime = currentPhaseTime;
+
+        float currentHeading = peripherals->getHeading();
+        if (currentHeading >= 0.0f && _startHeading >= 0.0f) {
+            float headingDelta = currentHeading - _startHeading;
+
+            if (headingDelta > 180.0f)
+                headingDelta -= 360.0f;
+            else if (headingDelta < -180.0f)
+                headingDelta += 360.0f;
+
+            _currentRotation = headingDelta;
+        }
+
+        bool distanceReached = _targetDistance == 0.0f || _currentDistance >= (_targetDistance - DISTANCE_TOLERANCE);
+        bool headingReached = _targetHeading == 0.0f || abs(_currentRotation - _targetHeading) <= HEADING_TOLERANCE;
+        bool rotationReached = _targetRotation == 0.0f || abs(_currentRotation - _targetRotation) <= ROTATION_TOLERANCE;
+
+        if (distanceReached && headingReached && rotationReached) {
+            _isComplete = true;
+
+            if (currentState) {
+                CommandMsg stopCommand = {0};
+                stopCommand.h = KinConfig::default_body_height;
+                stopCommand.s = WALK_SPEED;
+                currentState->handleCommand(stopCommand);
+            }
+
+            ESP_LOGI("WalkSkill", "Walk completed: %.2fm/%.2fm, %.1f°/%.1f° rotation", _currentDistance,
+                     _targetDistance, _currentRotation, _targetRotation);
+            return;
+        }
+
+        CommandMsg walkCommand = {0};
+        walkCommand.h = WALK_SPEED;
+        walkCommand.s1 = WALK_SPEED;
+        walkCommand.s = WALK_SPEED;
+
+        if (_targetDistance > 0.0f && !distanceReached) {
+            walkCommand.lx = WALK_SPEED;
+        } else if (_targetDistance < 0.0f && !distanceReached) {
+            walkCommand.lx = -WALK_SPEED;
+        }
+
+        if (_targetHeading != 0.0f && !headingReached) {
+            if (_currentRotation < _targetHeading) {
+                walkCommand.ly = WALK_SPEED;
+            } else {
+                walkCommand.ly = -WALK_SPEED;
+            }
+        }
+
+        if (_targetRotation != 0.0f && !rotationReached) {
+            if (_targetRotation > 0.0f) {
+                walkCommand.rx = WALK_SPEED;
+            } else {
+                walkCommand.rx = -WALK_SPEED;
+            }
+        }
+
+        if (currentState) {
+            currentState->handleCommand(walkCommand);
+        }
+    }
+
+    bool isComplete() const override { return _isComplete; }
+
+    void reset() override {
+        _isActive = false;
+        _isComplete = false;
+        _currentDistance = 0.0f;
+        _currentRotation = 0.0f;
+        _lastPhaseTime = 0.0f;
+        _stepCount = 0;
+        _estimatedStepLength = 0.0f;
+        _startTime = 0;
+    }
+
+    MotionState* getRequiredState() override { return reinterpret_cast<MotionState*>(_walkState); }
+
+    void setWalkState(WalkState* walkState) { _walkState = walkState; }
+};

esp32/include/motion_states/walk_state.h

@@ -71,6 +71,9 @@ class WalkState : public MotionState {
     WalkState() = default;
     const char *name() const override { return "Bezier"; }
 
+    float getPhaseTime() const { return phase_time; }
+    float getStepLength() const { return step_length; }
+
     void set_mode_crawl(float duty = 0.85f, std::array<int, 4> order = {3, 0, 2, 1}) {
         mode = WALK_GAIT::CRAWL;
         speed_factor = 0.5;

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() {