🎨 Simplifies spin skill

esp32/include/motion.h

@@ -113,42 +113,26 @@ class MotionService {
         if (ges != gesture_t::eGestureNone) {
             ESP_LOGI("Motion", "Gesture: %d", ges);
 
-            // Check if this gesture maps to a skill
+            if (ges == gesture_t::eGestureUp) {
-            if (ges == gesture_t::eGestureClockwise || ges == gesture_t::eGestureAntiClockwise) {
+                skillManager.clearQueue();
-                skillManager.queueGestureSkill(ges);
+                if (!state) {
-                return; // Let skill manager handle state transitions
+                    _servoController->activate();
-            }
+                    setState(&restState);
-
+                } else if (state == &restState)
-            // Handle basic gestures that don't require skills
+                    setState(&standState);
-            switch (ges) {
+                else if (state == &standState)
-                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;
+            } else if (ges == gesture_t::eGestureDown) {
-                default: break;
+                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);
             }
         }
     }
@@ -160,10 +144,8 @@ class MotionService {
         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) {

esp32/include/motion_skills/skill_manager.h

@@ -2,6 +2,7 @@
 
 #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>
@@ -28,14 +29,40 @@ class SkillManager {
         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:
-                skill = std::make_unique<SpinAroundSkill>(true);
+                // Rotate 90 degrees clockwise
-                static_cast<SpinAroundSkill*>(skill.get())->setWalkState(_walkState);
+                skill = std::make_unique<WalkSkill>(0.0f, 0.0f, 90.0f);
+                static_cast<WalkSkill*>(skill.get())->setWalkState(_walkState);
                 break;
 
             case gesture_t::eGestureAntiClockwise:
-                skill = std::make_unique<SpinAroundSkill>(false);
+                // Rotate 90 degrees counter-clockwise
-                static_cast<SpinAroundSkill*>(skill.get())->setWalkState(_walkState);
+                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

esp32/include/motion_skills/spin_skill.h

@@ -1,120 +1,13 @@
 #pragma once
 
-#include <motion_skills/skill.h>
+#include <motion_skills/walk_skill.h>
-#include <utils/math_utils.h>
-#include <esp_log.h>
 
-// Forward declaration to avoid circular dependency
+class SpinAroundSkill : public WalkSkill {
-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) {}
+    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"; }
-
-    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/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;