✨ Adds skilmanager and spin skill

2025-09-10 15:59:41 +02:00
parent 26c36b8302
commit 3a401abfab
5 changed files with 306 additions and 6 deletions
@@ -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;
+};
@@ -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());
+    }
+};
@@ -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; }
+};