From 893ee3865182711d5a6240ffa202570f4c6c5cd0 Mon Sep 17 00:00:00 2001 From: Forairaaaaa Date: Thu, 11 Jun 2026 10:45:02 +0800 Subject: [PATCH] - Added current monitoring and stall protection for the Y-axis servo - Optimized the screen touch debouncing under AI.Agent mode - Bump version --- firmware/CMakeLists.txt | 2 +- firmware/main/hal/board/stackchan_display.cc | 7 + firmware/main/hal/hal_servo.cpp | 163 +++++++++++++++++-- firmware/main/stackchan/motion/servo.cpp | 7 + firmware/main/stackchan/motion/servo.h | 1 + 5 files changed, 166 insertions(+), 14 deletions(-) diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index f317ef9..d6655fc 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -2,7 +2,7 @@ # CMakeLists in this exact order for cmake to work correctly cmake_minimum_required(VERSION 3.16) -set(PROJECT_VER "1.4.1") +set(PROJECT_VER "1.4.2") add_definitions(-DFIRMWARE_VERSION=\"${PROJECT_VER}\") # Add this line to disable the specific warning diff --git a/firmware/main/hal/board/stackchan_display.cc b/firmware/main/hal/board/stackchan_display.cc index e280eeb..7459888 100644 --- a/firmware/main/hal/board/stackchan_display.cc +++ b/firmware/main/hal/board/stackchan_display.cc @@ -254,7 +254,14 @@ void StackChanAvatarDisplay::SetupUI() auto avatar = std::make_unique(); avatar->init(lv_screen_active()); avatar->getPanel()->onClick().connect([]() { + static uint32_t last_toggle_tick = 0; + const uint32_t now = GetHAL().millis(); + if (last_toggle_tick != 0 && now - last_toggle_tick < 2000) { + return; + } + if (hal_bridge::is_xiaozhi_ready()) { + last_toggle_tick = now; hal_bridge::toggle_xiaozhi_chat_state(); } }); diff --git a/firmware/main/hal/hal_servo.cpp b/firmware/main/hal/hal_servo.cpp index 6acaadb..a2429e2 100644 --- a/firmware/main/hal/hal_servo.cpp +++ b/firmware/main/hal/hal_servo.cpp @@ -23,18 +23,20 @@ struct ServoConfig_t { std::string settingNs; std::string settingZeroPositionKey; bool enablePwmMode = false; + bool enableStallProtection = false; }; class ScsServo : public Servo { public: static inline const std::string _tag = "ScsServo"; - ScsServo(const ServoConfig_t& config) : _config(config) + ScsServo(const ServoConfig_t& config) : _config(config), _runtime_raw_pos_limit(config.rawPosLimit) { } void init() override { + reset_runtime_limits(); set_angle_limit(_config.angleLimit); get_zero_pos_from_nvs(); Servo::init(); @@ -72,10 +74,14 @@ public: void set_angle_impl(int angle) override { int mapped_angle = _zero_pos + angle * 16 / 5 / 10; // 一步对应 0.3125度, 0.3125 = 5/16 - mapped_angle = uitk::clamp(mapped_angle, _config.rawPosLimit.x, _config.rawPosLimit.y); + mapped_angle = uitk::clamp(mapped_angle, _runtime_raw_pos_limit.x, _runtime_raw_pos_limit.y); // mclog::tagInfo(_tag, "id: {} mapped angle: {}", _id, mapped_angle); + if (update_stall_protection(mapped_angle)) { + return; + } + check_mode(Mode::Position); _scs_bus.WritePos(_config.id, mapped_angle, 20, 0); } @@ -85,16 +91,13 @@ public: int current_pos = _scs_bus.ReadPos(_config.id); if (!is_raw_pos_valid(current_pos)) { int fallback_angle = uitk::clamp(Servo::getCurrentAngle(), getAngleLimit().x, getAngleLimit().y); - mclog::tagWarn(_tag, - "id: {} ignore invalid current pos: {}, fallback angle: {}", - _config.id, - current_pos, + mclog::tagWarn(_tag, "id: {} ignore invalid current pos: {}, fallback angle: {}", _config.id, current_pos, fallback_angle); return fallback_angle; } - int angle = raw_pos_to_angle(current_pos); - angle = uitk::clamp(angle, getAngleLimit().x, getAngleLimit().y); + int angle = raw_pos_to_angle(current_pos); + angle = uitk::clamp(angle, getAngleLimit().x, getAngleLimit().y); // mclog::tagInfo(_tag, "id: {} current pos: {} angle: {}", _id, current_pos, angle); return angle; } @@ -124,15 +127,13 @@ public: { int current_pos = _scs_bus.ReadPos(_config.id); if (!is_raw_pos_valid(current_pos)) { - mclog::tagWarn(_tag, - "id: {} ignore invalid zero calibration pos: {}, keep zero pos: {}", - _config.id, - current_pos, - _zero_pos); + mclog::tagWarn(_tag, "id: {} ignore invalid zero calibration pos: {}, keep zero pos: {}", _config.id, + current_pos, _zero_pos); return; } _zero_pos = current_pos; + reset_runtime_limits(); Settings settings(_config.settingNs, true); settings.SetInt(_config.settingZeroPositionKey, _zero_pos); @@ -143,6 +144,7 @@ public: void resetZeroCalibration() override { _zero_pos = _config.defaultZeroPos; + reset_runtime_limits(); Settings settings(_config.settingNs, true); settings.SetInt(_config.settingZeroPositionKey, _zero_pos); @@ -168,9 +170,32 @@ private: enum class Mode { Position = 0, PWM = 1 }; ServoConfig_t _config; + Vector2i _runtime_raw_pos_limit; int _zero_pos = 0; Mode _current_mode = Mode::Position; + static constexpr uint32_t kStallFeedbackIntervalMs = 50; + static constexpr int kStallMinTargetDeltaRaw = 8; + static constexpr int kStallMaxPositionDeltaRaw = 1; + static constexpr int kStallCurrentRiseThreshold = 80; + static constexpr int kStallLoadRiseThreshold = 150; + static constexpr int kStallCurrentAbsThreshold = 350; + static constexpr int kStallLoadAbsThreshold = 650; + static constexpr int kStallConfirmSamples = 2; + + uint32_t _last_stall_check_tick = 0; + int _last_stall_raw_pos = 0; + int _last_stall_current_abs = 0; + int _last_stall_load_abs = 0; + int _last_stall_direction = 0; + int _stall_confirm_count = 0; + bool _last_stall_feedback_valid = false; + + static int abs_int(int value) + { + return value < 0 ? -value : value; + } + bool is_raw_pos_valid(int raw_pos) const { return raw_pos >= _config.rawPosLimit.x && raw_pos <= _config.rawPosLimit.y; @@ -181,6 +206,117 @@ private: return (raw_pos - _zero_pos) * 5 * 10 / 16; } + void reset_runtime_limits() + { + _runtime_raw_pos_limit = _config.rawPosLimit; + set_angle_limit(_config.angleLimit); + reset_stall_detection(); + } + + void reset_stall_detection() + { + _last_stall_feedback_valid = false; + _last_stall_direction = 0; + _stall_confirm_count = 0; + } + + bool update_stall_protection(int target_raw_pos) + { + if (!_config.enableStallProtection) { + return false; + } + + const uint32_t now = GetHAL().millis(); + if (now - _last_stall_check_tick < kStallFeedbackIntervalMs) { + return false; + } + _last_stall_check_tick = now; + + if (_scs_bus.FeedBack(_config.id) < 0) { + reset_stall_detection(); + return false; + } + + const int current_raw_pos = _scs_bus.ReadPos(-1); + const int current_abs = abs_int(_scs_bus.ReadCurrent(-1)); + const int load_abs = abs_int(_scs_bus.ReadLoad(-1)); + + if (!is_raw_pos_valid(current_raw_pos)) { + reset_stall_detection(); + return false; + } + + const int target_delta = target_raw_pos - current_raw_pos; + if (abs_int(target_delta) < kStallMinTargetDeltaRaw) { + reset_stall_detection(); + return false; + } + + const int direction = target_delta > 0 ? 1 : -1; + if (_last_stall_feedback_valid && direction == _last_stall_direction) { + const int pos_delta = abs_int(current_raw_pos - _last_stall_raw_pos); + const bool position_stuck = pos_delta <= kStallMaxPositionDeltaRaw; + const bool current_spike = current_abs >= kStallCurrentAbsThreshold || + current_abs - _last_stall_current_abs >= kStallCurrentRiseThreshold; + const bool load_spike = load_abs >= kStallLoadAbsThreshold || + load_abs - _last_stall_load_abs >= kStallLoadRiseThreshold; + + if (position_stuck && (current_spike || load_spike)) { + _stall_confirm_count++; + } else if (pos_delta > kStallMaxPositionDeltaRaw) { + _stall_confirm_count = 0; + } + } else { + _stall_confirm_count = 0; + } + + _last_stall_raw_pos = current_raw_pos; + _last_stall_current_abs = current_abs; + _last_stall_load_abs = load_abs; + _last_stall_direction = direction; + _last_stall_feedback_valid = true; + + if (_stall_confirm_count < kStallConfirmSamples) { + return false; + } + + handle_stall(current_raw_pos, direction, current_abs, load_abs); + return true; + } + + void handle_stall(int raw_pos, int direction, int current_abs, int load_abs) + { + int angle = raw_pos_to_angle(raw_pos); + angle = uitk::clamp(angle, _config.angleLimit.x, _config.angleLimit.y); + + auto angle_limit = getAngleLimit(); + if (direction > 0) { + if (raw_pos < _runtime_raw_pos_limit.y) { + _runtime_raw_pos_limit.y = raw_pos; + } + if (angle < angle_limit.y) { + angle_limit.y = angle; + } + } else { + if (raw_pos > _runtime_raw_pos_limit.x) { + _runtime_raw_pos_limit.x = raw_pos; + } + if (angle > angle_limit.x) { + angle_limit.x = angle; + } + } + set_angle_limit(angle_limit); + stop_motion_at_angle(angle); + reset_stall_detection(); + + check_mode(Mode::Position); + _scs_bus.WritePos(_config.id, raw_pos, 20, 0); + + mclog::tagWarn(_tag, + "id: {} stall detected, raw: {}, angle: {}, dir: {}, current: {}, load: {}, limit: [{}, {}]", + _config.id, raw_pos, angle, direction, current_abs, load_abs, angle_limit.x, angle_limit.y); + } + void check_mode(Mode targetMode) { if (targetMode == _current_mode) { @@ -214,6 +350,7 @@ void Hal::servo_init() pitch_servo_config.rawPosLimit = Vector2i(0, 1000); pitch_servo_config.settingNs = "servo"; pitch_servo_config.settingZeroPositionKey = "zero_pos_2"; + pitch_servo_config.enableStallProtection = true; auto yaw_servo = std::make_unique(yaw_servo_config); auto pitch_servo = std::make_unique(pitch_servo_config); diff --git a/firmware/main/stackchan/motion/servo.cpp b/firmware/main/stackchan/motion/servo.cpp index e999cf9..cfbefe0 100644 --- a/firmware/main/stackchan/motion/servo.cpp +++ b/firmware/main/stackchan/motion/servo.cpp @@ -116,6 +116,13 @@ void Servo::update_angle_anim_target(int angle) _snap_to_target_on_rest = true; } +void Servo::stop_motion_at_angle(int angle) +{ + angle = uitk::clamp(angle, _angle_limit.x, _angle_limit.y); + _angle_anim.teleport(angle); + _snap_to_target_on_rest = false; +} + uitk::SpringOptions_t Servo::map_speed_to_spring_options(int speed) { speed = uitk::clamp(speed, 0, 1000); diff --git a/firmware/main/stackchan/motion/servo.h b/firmware/main/stackchan/motion/servo.h index 9fac5ff..f621c64 100644 --- a/firmware/main/stackchan/motion/servo.h +++ b/firmware/main/stackchan/motion/servo.h @@ -177,6 +177,7 @@ protected: void apply_default_spring_options(); void update_angle_anim_target(int angle); + void stop_motion_at_angle(int angle); uitk::SpringOptions_t map_speed_to_spring_options(int speed); };