From 3a401abfab331cea34c2f7f91f1ed01c9faa285d Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Wed, 10 Sep 2025 15:59:41 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Adds=20skilmanager=20and=20spin=20s?= =?UTF-8?q?kill?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/motion.h | 61 +++++++++- esp32/include/motion_skills/skill.h | 27 +++++ esp32/include/motion_skills/skill_manager.h | 96 ++++++++++++++++ esp32/include/motion_skills/spin_skill.h | 120 ++++++++++++++++++++ esp32/include/peripherals/peripherals.h | 8 ++ 5 files changed, 306 insertions(+), 6 deletions(-) create mode 100644 esp32/include/motion_skills/skill.h create mode 100644 esp32/include/motion_skills/skill_manager.h create mode 100644 esp32/include/motion_skills/spin_skill.h diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 1dd80ba..8638d04 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #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}; diff --git a/esp32/include/motion_skills/skill.h b/esp32/include/motion_skills/skill.h new file mode 100644 index 0000000..04adb5d --- /dev/null +++ b/esp32/include/motion_skills/skill.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include +#include +#include + +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; +}; diff --git a/esp32/include/motion_skills/skill_manager.h b/esp32/include/motion_skills/skill_manager.h new file mode 100644 index 0000000..3e06091 --- /dev/null +++ b/esp32/include/motion_skills/skill_manager.h @@ -0,0 +1,96 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +class SkillManager { + private: + std::queue> _skillQueue; + std::unique_ptr _currentSkill; + WalkState* _walkState = nullptr; + + public: + SkillManager() = default; + + void setWalkState(WalkState* walkState) { _walkState = walkState; } + + void queueSkill(std::unique_ptr 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 = nullptr; + + switch (gesture) { + case gesture_t::eGestureClockwise: + skill = std::make_unique(true); + static_cast(skill.get())->setWalkState(_walkState); + break; + + case gesture_t::eGestureAntiClockwise: + skill = std::make_unique(false); + static_cast(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()); + } +}; diff --git a/esp32/include/motion_skills/spin_skill.h b/esp32/include/motion_skills/spin_skill.h new file mode 100644 index 0000000..370362c --- /dev/null +++ b/esp32/include/motion_skills/spin_skill.h @@ -0,0 +1,120 @@ +#pragma once + +#include +#include +#include + +// 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; } +}; diff --git a/esp32/include/peripherals/peripherals.h b/esp32/include/peripherals/peripherals.h index e4c412e..6bfa717 100644 --- a/esp32/include/peripherals/peripherals.h +++ b/esp32/include/peripherals/peripherals.h @@ -209,6 +209,14 @@ class Peripherals : public StatefulService { 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 endpoint; void emitIMU() {