From 7b90339b78657cc778cc6c7c0131a644b0f08c5e Mon Sep 17 00:00:00 2001 From: John Malone Date: Tue, 19 May 2026 06:45:42 -0500 Subject: [PATCH] test: add firmware servo speed coverage --- firmware/README.md | 10 +++ firmware/main/stackchan/motion/servo.cpp | 38 ++-------- firmware/main/stackchan/motion/servo_math.cpp | 45 +++++++++++ firmware/main/stackchan/motion/servo_math.h | 21 +++++ firmware/tests/CMakeLists.txt | 18 +++++ firmware/tests/servo_math_test.cpp | 76 +++++++++++++++++++ 6 files changed, 178 insertions(+), 30 deletions(-) create mode 100644 firmware/main/stackchan/motion/servo_math.cpp create mode 100644 firmware/main/stackchan/motion/servo_math.h create mode 100644 firmware/tests/CMakeLists.txt create mode 100644 firmware/tests/servo_math_test.cpp diff --git a/firmware/README.md b/firmware/README.md index ffc20f2f..76515f3c 100644 --- a/firmware/README.md +++ b/firmware/README.md @@ -17,6 +17,16 @@ python3 ./fetch_repos.py idf.py build ``` +### Host-side tests + +Servo motion profiling helpers can be tested without ESP-IDF hardware: + +```bash +cmake -S tests -B build-host-tests +cmake --build build-host-tests +ctest --test-dir build-host-tests --output-on-failure +``` + ### Flash ```bash diff --git a/firmware/main/stackchan/motion/servo.cpp b/firmware/main/stackchan/motion/servo.cpp index e999cf93..fcd776b2 100644 --- a/firmware/main/stackchan/motion/servo.cpp +++ b/firmware/main/stackchan/motion/servo.cpp @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ #include "servo.h" +#include "servo_math.h" #include using namespace uitk; @@ -118,37 +119,14 @@ void Servo::update_angle_anim_target(int angle) uitk::SpringOptions_t Servo::map_speed_to_spring_options(int speed) { - speed = uitk::clamp(speed, 0, 1000); - - // 1. 计算 Stiffness (刚度) - // 使用二次方映射: k = k_min + (speed/1000)^2 * k_range - // 当 speed=500 时，k 约为 10 + 0.25 * 640 = 170 - float k_min = 10.0f; - float k_max = 650.0f; - float normalizedSpeed = speed / 1000.0f; - float stiffness = k_min + (normalizedSpeed * normalizedSpeed) * (k_max - k_min); - - // 2. 计算 Damping (阻尼) - // 为了保持临界阻尼(无过冲，最快稳定)，公式为 d = 2 * sqrt(m * k) - // 如果想要带一点点弹性感(bounce)，可以将系数从 2.0 降到 1.5~1.8 - float mass = 1.0f; - float damping = 2.0f * sqrtf(mass * stiffness); - - // 3. 构造选项 + auto profile = calculateServoSpringProfile(speed); + uitk::SpringOptions_t options = _default_spring_options; - options.stiffness = stiffness; - options.damping = damping; - options.mass = mass; - - // 4. 动态调整静止阈值 - // 高速时阈值大一点可以防止由于离散计算导致的微小抖动 - if (speed > 800) { - options.restDelta = 0.5f; - options.restSpeed = 0.5f; - } else { - options.restDelta = 0.1f; - options.restSpeed = 0.1f; - } + options.stiffness = profile.stiffness; + options.damping = profile.damping; + options.mass = profile.mass; + options.restDelta = profile.restDelta; + options.restSpeed = profile.restSpeed; return options; } diff --git a/firmware/main/stackchan/motion/servo_math.cpp b/firmware/main/stackchan/motion/servo_math.cpp new file mode 100644 index 00000000..721982cb --- /dev/null +++ b/firmware/main/stackchan/motion/servo_math.cpp @@ -0,0 +1,45 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include "servo_math.h" +#include +#include + +namespace stackchan::motion { + +int clampServoSpeed(int speed) +{ + return std::clamp(speed, 0, 1000); +} + +ServoSpringProfile calculateServoSpringProfile(int speed) +{ + speed = clampServoSpeed(speed); + + float kMin = 10.0f; + float kMax = 650.0f; + float normalizedSpeed = speed / 1000.0f; + float stiffness = kMin + (normalizedSpeed * normalizedSpeed) * (kMax - kMin); + + float mass = 1.0f; + float damping = 2.0f * std::sqrt(mass * stiffness); + + ServoSpringProfile profile{ + .stiffness = stiffness, + .damping = damping, + .mass = mass, + .restSpeed = 0.1f, + .restDelta = 0.1f, + }; + + if (speed > 800) { + profile.restDelta = 0.5f; + profile.restSpeed = 0.5f; + } + + return profile; +} + +} // namespace stackchan::motion diff --git a/firmware/main/stackchan/motion/servo_math.h b/firmware/main/stackchan/motion/servo_math.h new file mode 100644 index 00000000..6a267f5a --- /dev/null +++ b/firmware/main/stackchan/motion/servo_math.h @@ -0,0 +1,21 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#pragma once + +namespace stackchan::motion { + +struct ServoSpringProfile { + float stiffness; + float damping; + float mass; + float restSpeed; + float restDelta; +}; + +int clampServoSpeed(int speed); +ServoSpringProfile calculateServoSpringProfile(int speed); + +} // namespace stackchan::motion diff --git a/firmware/tests/CMakeLists.txt b/firmware/tests/CMakeLists.txt new file mode 100644 index 00000000..a60c41db --- /dev/null +++ b/firmware/tests/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.16) + +project(stackchan_firmware_tests LANGUAGES CXX) + +enable_testing() + +add_executable(servo_math_test + servo_math_test.cpp + ../main/stackchan/motion/servo_math.cpp +) + +target_include_directories(servo_math_test PRIVATE + ../main +) + +target_compile_features(servo_math_test PRIVATE cxx_std_17) + +add_test(NAME servo_math_test COMMAND $) diff --git a/firmware/tests/servo_math_test.cpp b/firmware/tests/servo_math_test.cpp new file mode 100644 index 00000000..6571b9a7 --- /dev/null +++ b/firmware/tests/servo_math_test.cpp @@ -0,0 +1,76 @@ +/* + * SPDX-FileCopyrightText: 2026 M5Stack Technology CO LTD + * + * SPDX-License-Identifier: MIT + */ +#include +#include +#include +#include + +namespace { + +using stackchan::motion::ServoSpringProfile; +using stackchan::motion::calculateServoSpringProfile; +using stackchan::motion::clampServoSpeed; + +void expectEqual(int actual, int expected, const char* label) +{ + if (actual != expected) { + std::cerr << label << ": expected " << expected << ", got " << actual << '\n'; + std::exit(1); + } +} + +void expectNear(float actual, float expected, float tolerance, const char* label) +{ + if (std::fabs(actual - expected) > tolerance) { + std::cerr << label << ": expected " << expected << ", got " << actual << '\n'; + std::exit(1); + } +} + +void testSpeedClamp() +{ + expectEqual(clampServoSpeed(-10), 0, "speed clamp low"); + expectEqual(clampServoSpeed(500), 500, "speed clamp mid"); + expectEqual(clampServoSpeed(1200), 1000, "speed clamp high"); +} + +void testProfileAtDefaultMidSpeed() +{ + ServoSpringProfile profile = calculateServoSpringProfile(500); + expectNear(profile.stiffness, 170.0f, 0.01f, "mid stiffness"); + expectNear(profile.damping, 26.0768f, 0.01f, "mid damping"); + expectNear(profile.mass, 1.0f, 0.0f, "mid mass"); + expectNear(profile.restSpeed, 0.1f, 0.0f, "mid rest speed"); + expectNear(profile.restDelta, 0.1f, 0.0f, "mid rest delta"); +} + +void testProfileAtHighSpeedUsesRelaxedRestThresholds() +{ + ServoSpringProfile profile = calculateServoSpringProfile(900); + expectNear(profile.stiffness, 528.4f, 0.01f, "high stiffness"); + expectNear(profile.damping, 45.974f, 0.02f, "high damping"); + expectNear(profile.restSpeed, 0.5f, 0.0f, "high rest speed"); + expectNear(profile.restDelta, 0.5f, 0.0f, "high rest delta"); +} + +void testProfileClampMatchesEndpoints() +{ + ServoSpringProfile low = calculateServoSpringProfile(-1); + ServoSpringProfile high = calculateServoSpringProfile(1001); + expectNear(low.stiffness, 10.0f, 0.0f, "low stiffness"); + expectNear(high.stiffness, 650.0f, 0.0f, "high stiffness"); +} + +} // namespace + +int main() +{ + testSpeedClamp(); + testProfileAtDefaultMidSpeed(); + testProfileAtHighSpeedUsesRelaxedRestThresholds(); + testProfileClampMatchesEndpoints(); + return 0; +}