diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 8638d04..38bde49 100644 --- a/esp32/include/motion.h +++ b/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::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 + 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); - break; - default: break; + } 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); } } } @@ -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) { diff --git a/esp32/include/motion_skills/skill_manager.h b/esp32/include/motion_skills/skill_manager.h index 3e06091..be24cb7 100644 --- a/esp32/include/motion_skills/skill_manager.h +++ b/esp32/include/motion_skills/skill_manager.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -28,14 +29,40 @@ class SkillManager { std::unique_ptr skill = nullptr; switch (gesture) { + case gesture_t::eGestureLeft: + // Walk 1m left (90 degrees heading) + skill = std::make_unique(1.0f, 90.0f, 0.0f); + static_cast(skill.get())->setWalkState(_walkState); + break; + + case gesture_t::eGestureRight: + // Walk 1m right (-90 degrees heading) + skill = std::make_unique(1.0f, -90.0f, 0.0f); + static_cast(skill.get())->setWalkState(_walkState); + break; + + case gesture_t::eGestureUp: + // Walk 1m forward (0 degrees heading) + skill = std::make_unique(1.0f, 0.0f, 0.0f); + static_cast(skill.get())->setWalkState(_walkState); + break; + + case gesture_t::eGestureDown: + // Walk 1m backward (180 degrees heading) + skill = std::make_unique(-1.0f, 0.0f, 0.0f); + static_cast(skill.get())->setWalkState(_walkState); + break; + case gesture_t::eGestureClockwise: - skill = std::make_unique(true); - static_cast(skill.get())->setWalkState(_walkState); + // Rotate 90 degrees clockwise + skill = std::make_unique(0.0f, 0.0f, 90.0f); + static_cast(skill.get())->setWalkState(_walkState); break; case gesture_t::eGestureAntiClockwise: - skill = std::make_unique(false); - static_cast(skill.get())->setWalkState(_walkState); + // Rotate 90 degrees counter-clockwise + skill = std::make_unique(0.0f, 0.0f, -90.0f); + static_cast(skill.get())->setWalkState(_walkState); break; default: return; // No skill mapped to this gesture diff --git a/esp32/include/motion_skills/spin_skill.h b/esp32/include/motion_skills/spin_skill.h index 370362c..d1348ca 100644 --- a/esp32/include/motion_skills/spin_skill.h +++ b/esp32/include/motion_skills/spin_skill.h @@ -1,120 +1,13 @@ #pragma once -#include -#include -#include +#include -// Forward declaration to avoid circular dependency -class WalkState; - -class SpinAroundSkill : public Skill { +class SpinAroundSkill : public WalkSkill { 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; } }; diff --git a/esp32/include/motion_skills/walk_skill.h b/esp32/include/motion_skills/walk_skill.h new file mode 100644 index 0000000..055da8b --- /dev/null +++ b/esp32/include/motion_skills/walk_skill.h @@ -0,0 +1,155 @@ +#pragma once + +#include +#include +#include + +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(_walkState); } + + void setWalkState(WalkState* walkState) { _walkState = walkState; } +}; diff --git a/esp32/include/motion_states/walk_state.h b/esp32/include/motion_states/walk_state.h index 1bcdf44..d95e42a 100644 --- a/esp32/include/motion_states/walk_state.h +++ b/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 order = {3, 0, 2, 1}) { mode = WALK_GAIT::CRAWL; speed_factor = 0.5;