refactor: 重构架构并整理仓库

2026-09-23 10:56:57 +08:00
parent 680adae339
commit f8aeafdfc2
101 changed files with 14417 additions and 11399 deletions
@@ -0,0 +1,16 @@
+[package]
+name = "motor-model"
+description = "DC motor physical simulation model"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+motor-protocol = { workspace = true }
+
+[dev-dependencies]
+approx = { workspace = true }
@@ -0,0 +1,426 @@
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+//! DC motor physical models.
+
+pub mod dc_motor {
+    //! 被控对象模型：一阶转速 + 摩擦 / 延迟 / 负载。
+    //!
+    //! 电调坐标系（正电流 → 正转速），逐拍离散（`dt` 秒）：
+    //!
+    //! 1. **命令延迟**：`current_raw` 进 FIFO，`delay_ticks` 拍后才真正作用（模拟
+    //!    链路 + 采样延迟；`0` 表示立即作用）。
+    //! 2. **摩擦**：先由 `u` 与当前运动状态算出净转矩输入
+    //!    （静摩擦死区 → 库仑摩擦 → 黏滞阻尼，见 [`PlantMotor::step`]）。
+    //! 3. **一阶转速**：`rpm_target = clamp(net × gain, ±max_rpm) − load_rpm`，
+    //!    转速以时间常数 `tau` 一阶趋近（`alpha = 1 − exp(−dt/tau)`）。
+    //! 4. **编码器积分与量化**：机械角按转速积分并对 8192 取模；反馈帧是
+    //!    `EscFeedback`，因此角度被截断为 `u16`、转速被截断为 `i16`——量化是
+    //!    协议类型自带的，不需要额外开关。
+    //!
+    //! [`PlantParams::gm3508`] 复现最初的**无摩擦一阶模型**（`chassisd --mock`
+    //! 与闭环测试依赖它，行为逐位不变）；[`PlantParams::gm3508_bench`] 是带摩擦
+    //! 与延迟的**台架量级**预设，供自动整定使用。
+
+    use std::collections::VecDeque;
+
+    use motor_protocol::{ENCODER_TICKS, EscFeedback};
+
+    /// 转速绝对值低于该值（rpm）视为静止——静摩擦判别用。
+    const STICK_RPM_EPS: f32 = 1.0;
+
+    /// 被控对象参数（电调坐标系）。
+    #[derive(Debug, Clone, Copy, PartialEq)]
+    pub struct PlantParams {
+        /// 空载增益（rpm / raw）：`GM3508` 实测约 0.565（≈ 9254 / 16384）。
+        pub gain_rpm_per_raw: f32,
+        /// 机电时间常数（秒）。
+        pub tau_s: f32,
+        /// 转速上限（rpm）。
+        pub max_rpm: f32,
+        /// 静摩擦阈值（raw）：静止且 |u| ≤ 该值时不产生任何运动（stick）。
+        pub static_raw: f32,
+        /// 库仑摩擦（raw）：与运动方向相反的恒定阻力。
+        pub coulomb_raw: f32,
+        /// 黏滞阻尼（raw / rpm）。
+        pub viscous_raw_per_rpm: f32,
+        /// 命令传输延迟（拍，`dt` 的整数倍）。
+        pub delay_ticks: u32,
+        /// 恒定外载（折算成 rpm，直接被目标转速减去）。
+        pub load_rpm: f32,
+    }
+
+    impl PlantParams {
+        /// 无摩擦、无延迟的理想一阶模型（与历史 `PlantMotor::gm3508` 逐位一致）。
+        #[must_use]
+        pub const fn gm3508() -> Self {
+            Self {
+                gain_rpm_per_raw: 9254.0 / 16384.0,
+                tau_s: 0.12,
+                max_rpm: 9254.0,
+                static_raw: 0.0,
+                coulomb_raw: 0.0,
+                viscous_raw_per_rpm: 0.0,
+                delay_ticks: 0,
+                load_rpm: 0.0,
+            }
+        }
+
+        /// 台架量级预设：在 [`Self::gm3508`] 上叠加静摩擦 / 库仑摩擦 / 黏滞阻尼
+        /// 与 2 拍命令延迟。
+        ///
+        /// 摩擦量级取自 `docs/architecture.md` 与台架记录的空载实测（起动需
+        /// 400~600 raw），**是量级估计而非辨识结果**；需要更贴合实车时可用
+        /// [`Self::with_friction`] / [`Self::with_delay_ticks`] 覆盖。
+        #[must_use]
+        pub const fn gm3508_bench() -> Self {
+            let mut params = Self::gm3508();
+            params.static_raw = 450.0;
+            params.coulomb_raw = 500.0;
+            params.viscous_raw_per_rpm = 0.05;
+            params.delay_ticks = 2;
+            params
+        }
+
+        /// 设置摩擦三件套（静摩擦阈值 / 库仑 / 黏滞）。
+        #[must_use]
+        pub const fn with_friction(
+            mut self,
+            static_raw: f32,
+            coulomb_raw: f32,
+            viscous_raw_per_rpm: f32,
+        ) -> Self {
+            self.static_raw = static_raw;
+            self.coulomb_raw = coulomb_raw;
+            self.viscous_raw_per_rpm = viscous_raw_per_rpm;
+            self
+        }
+
+        /// 设置命令延迟（拍）。
+        #[must_use]
+        pub const fn with_delay_ticks(mut self, ticks: u32) -> Self {
+            self.delay_ticks = ticks;
+            self
+        }
+
+        /// 设置恒定外载（rpm）。
+        #[must_use]
+        pub const fn with_load_rpm(mut self, rpm: f32) -> Self {
+            self.load_rpm = rpm;
+            self
+        }
+    }
+
+    /// 单台电机被控对象。`Clone` 便于在同一组参数上重放多条场景。
+    #[derive(Debug, Clone)]
+    pub struct PlantMotor {
+        params: PlantParams,
+        speed_rpm: f32,
+        angle_raw: f32,
+        /// 命令延迟 FIFO（`delay_ticks == 0` 时恒为空）。
+        delay_line: VecDeque<f32>,
+    }
+
+    impl PlantMotor {
+        /// 用给定参数创建静止的电机。
+        #[must_use]
+        pub fn new(params: PlantParams) -> Self {
+            Self { params, speed_rpm: 0.0, angle_raw: 0.0, delay_line: VecDeque::new() }
+        }
+
+        /// 理想 `GM3508`（无摩擦、无延迟）——历史默认，`--mock` 与闭环测试沿用。
+        #[must_use]
+        pub fn gm3508() -> Self {
+            Self::new(PlantParams::gm3508())
+        }
+
+        /// 当前参数。
+        #[must_use]
+        pub const fn params(&self) -> &PlantParams {
+            &self.params
+        }
+
+        /// 当前转子转速（rpm，电调侧）。
+        #[must_use]
+        pub const fn speed_rpm(&self) -> f32 {
+            self.speed_rpm
+        }
+
+        /// 当前机械角（编码器计数，`0..8192`）。
+        #[must_use]
+        pub const fn angle_raw(&self) -> f32 {
+            self.angle_raw
+        }
+
+        /// 回到静止初态（保留参数）。
+        pub fn reset(&mut self) {
+            self.speed_rpm = 0.0;
+            self.angle_raw = 0.0;
+            self.delay_line.clear();
+        }
+
+        /// 推进一个时间步，返回本步反馈帧内容。
+        ///
+        /// 摩擦模型（`net` 即折算到电流量纲的净驱动量）：
+        ///
+        /// - 运动时（`|rpm| > 1`）：`net = u − sign(rpm)·coulomb − viscous·rpm`；
+        /// - 静止但 `|u| > static`：`net = u − sign(u)·coulomb`（突破静摩擦）；
+        /// - 静止且 `|u| ≤ static`：`net = 0`（粘住）。
+        ///
+        /// `static = coulomb = viscous = 0` 时三个分支都退化为 `net = u`，
+        /// 即历史的一阶模型。
+        pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
+            let applied = self.apply_delay(f32::from(current_raw));
+            let net = self.net_torque_raw(applied);
+            let target = (net * self.params.gain_rpm_per_raw)
+                .clamp(-self.params.max_rpm, self.params.max_rpm)
+                - self.params.load_rpm;
+            if self.params.tau_s > 0.0 && dt.is_finite() && dt > 0.0 {
+                let alpha = 1.0 - (-dt / self.params.tau_s).exp();
+                self.speed_rpm += (target - self.speed_rpm) * alpha;
+            } else {
+                // 非正时间常数 = 无惯性：立即到位（防御非法 dt）。
+                self.speed_rpm = target;
+            }
+            let ticks = f32::from(ENCODER_TICKS);
+            self.angle_raw =
+                (self.angle_raw + self.speed_rpm / 60.0 * ticks * dt).rem_euclid(ticks);
+            EscFeedback {
+                angle_raw: self.angle_raw as u16,
+                speed_rpm: self.speed_rpm as i16,
+                current_raw,
+                temperature_c: 30,
+            }
+        }
+
+        /// 命令延迟：返回 `delay_ticks` 拍之前写入的命令（未填满时返回 0）。
+        fn apply_delay(&mut self, u: f32) -> f32 {
+            let depth = self.params.delay_ticks as usize;
+            if depth == 0 {
+                return u;
+            }
+            self.delay_line.push_back(u);
+            while self.delay_line.len() > depth {
+                if let Some(delayed) = self.delay_line.pop_front() {
+                    return delayed;
+                }
+            }
+            0.0
+        }
+
+        /// 折算到电流量纲的净驱动量（含静/库仑/黏滞摩擦）。
+        fn net_torque_raw(&self, u: f32) -> f32 {
+            let (static_raw, coulomb_raw, viscous) =
+                (self.params.static_raw, self.params.coulomb_raw, self.params.viscous_raw_per_rpm);
+            if self.speed_rpm.abs() > STICK_RPM_EPS {
+                let dir = if self.speed_rpm > 0.0 { 1.0 } else { -1.0 };
+                u - dir * coulomb_raw - viscous * self.speed_rpm
+            } else if u.abs() > static_raw {
+                let dir = if u > 0.0 { 1.0 } else { -1.0 };
+                u - dir * coulomb_raw
+            } else {
+                0.0
+            }
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        const DT: f32 = 0.001;
+
+        /// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
+        #[track_caller]
+        fn assert_close(actual: f32, expected: f32, what: &str) {
+            assert!(
+                (actual - expected).abs() < 1e-3,
+                "{what}: actual {actual}, expected {expected}"
+            );
+        }
+
+        #[test]
+        fn ideal_model_matches_legacy_first_order_response() {
+            // 与历史实现（无摩擦一阶模型）逐位一致的黄金值。
+            let mut plant = PlantMotor::gm3508();
+            let gain = 9254.0 / 16384.0;
+            let mut expected = 0.0f32;
+            for _ in 0..50 {
+                let target = (10_000f32 * gain).clamp(-9254.0, 9254.0);
+                let alpha = 1.0 - (-DT / 0.12).exp();
+                expected += (target - expected) * alpha;
+                let fb = plant.step(10_000, DT);
+                assert_eq!(fb.speed_rpm, expected as i16, "legacy step mismatch");
+            }
+            // 50 拍（50 ms）只走完约 34% 的一阶响应 → 约 1925 rpm（远未到稳态）。
+            // 精确数值由上面的逐拍比对保证，这里只锁定量级。
+            assert!((1900.0..1950.0).contains(&plant.speed_rpm()), "rpm = {}", plant.speed_rpm());
+        }
+
+        #[test]
+        fn zero_current_stays_still() {
+            let mut plant = PlantMotor::gm3508();
+            for _ in 0..100 {
+                let fb = plant.step(0, DT);
+                assert_eq!(fb.speed_rpm, 0);
+            }
+            assert!((plant.angle_raw()).abs() < f32::EPSILON);
+        }
+
+        #[test]
+        fn static_friction_holds_below_threshold() {
+            let params = PlantParams::gm3508().with_friction(450.0, 500.0, 0.0);
+            let mut plant = PlantMotor::new(params);
+            for _ in 0..1000 {
+                plant.step(400, DT);
+            }
+            assert_close(plant.speed_rpm(), 0.0, "低于静摩擦阈值的电流不得产生运动");
+        }
+
+        #[test]
+        fn breakaway_above_static_threshold_moves() {
+            let params = PlantParams::gm3508().with_friction(450.0, 500.0, 0.0);
+            let mut plant = PlantMotor::new(params);
+            for _ in 0..1000 {
+                plant.step(1000, DT);
+            }
+            // 净驱动 = 1000 − 500 = 500 raw → 稳态 ≈ 500 × 0.565 = 282 rpm。
+            let expected = 500.0 * (9254.0 / 16384.0);
+            assert!(
+                (plant.speed_rpm() - expected).abs() < 5.0,
+                "rpm = {} expected ≈ {expected}",
+                plant.speed_rpm()
+            );
+        }
+
+        #[test]
+        fn coulomb_friction_opposes_motion_in_both_directions() {
+            let params = PlantParams::gm3508().with_friction(0.0, 500.0, 0.0);
+            let mut forward = PlantMotor::new(params);
+            let mut backward = PlantMotor::new(params);
+            for _ in 0..2000 {
+                forward.step(2000, DT);
+                backward.step(-2000, DT);
+            }
+            assert!(forward.speed_rpm() > 0.0);
+            assert!(backward.speed_rpm() < 0.0);
+            assert!((forward.speed_rpm() + backward.speed_rpm()).abs() < 1.0, "对称性");
+        }
+
+        #[test]
+        fn delay_shifts_response_by_exact_ticks() {
+            let immediate = PlantParams::gm3508();
+            let delayed = PlantParams::gm3508().with_delay_ticks(3);
+            let mut a = PlantMotor::new(immediate);
+            let mut b = PlantMotor::new(delayed);
+            let mut a_trace = Vec::new();
+            let mut b_trace = Vec::new();
+            for _ in 0..100 {
+                a_trace.push(a.step(6000, DT).speed_rpm);
+                b_trace.push(b.step(6000, DT).speed_rpm);
+            }
+            // 延迟 3 拍：前 3 拍反馈恒为 0，之后 b[i] == a[i-3]。
+            assert_eq!(&b_trace[..3], &[0, 0, 0]);
+            for i in 3..100 {
+                assert_eq!(b_trace[i], a_trace[i - 3], "tick {i}");
+            }
+        }
+
+        #[test]
+        fn load_rpm_offsets_steady_state() {
+            let unloaded = PlantParams::gm3508();
+            let loaded = unloaded.with_load_rpm(200.0);
+            let mut a = PlantMotor::new(unloaded);
+            let mut b = PlantMotor::new(loaded);
+            for _ in 0..5000 {
+                a.step(16384, DT);
+                b.step(16384, DT);
+            }
+            assert!((a.speed_rpm() - 9254.0).abs() < 0.1, "空载稳态应顶到上限: {}", a.speed_rpm());
+            assert!(
+                (b.speed_rpm() - 9054.0).abs() < 0.1,
+                "外载直接把稳态拉低 200 rpm: {}",
+                b.speed_rpm()
+            );
+        }
+
+        #[test]
+        fn feedback_is_quantized_by_protocol_types() {
+            let mut plant = PlantMotor::gm3508();
+            for _ in 0..10_000 {
+                let fb = plant.step(9000, DT);
+                assert!(fb.angle_raw < 8192, "angle must stay inside one revolution");
+            }
+            let fb = plant.step(9000, DT);
+            // 内部转速是浮点，反馈按协议被截断为 i16——量化由类型自带。
+            assert_eq!(fb.speed_rpm, plant.speed_rpm() as i16);
+            assert_eq!(fb.temperature_c, 30);
+        }
+
+        #[test]
+        fn reset_returns_to_rest() {
+            let mut plant = PlantMotor::gm3508();
+            for _ in 0..100 {
+                plant.step(8000, DT);
+            }
+            assert!(plant.speed_rpm() > 0.0);
+            plant.reset();
+            assert_close(plant.speed_rpm(), 0.0, "复位后转速归零");
+            assert_close(plant.angle_raw(), 0.0, "复位后角度归零");
+        }
+    }
+}
+
+pub mod gm3508 {
+    //! GM3508 的被控对象参数（ARCHITECTURE.md §5.6 / §8.2）。
+    //!
+    //! 这里只放**有实测依据**的两组参数：
+    //!
+    //! - [`ideal_params`]：无摩擦、无延迟的理想一阶模型。历史闭环测试与
+    //!   `chassisd --mock` 依赖它，因此行为**逐位不变**；
+    //! - [`bench_params`]：在理想模型上叠加台架实测的静 / 库仑摩擦
+    //!   （400~600 raw 量级）与 2 拍命令延迟，是自动整定的默认被控对象。
+    //!
+    //! 数值口径见 `docs/chassis.md` §7.4 与 `config/tune.toml`。
+
+    use super::dc_motor::PlantParams;
+
+    /// 理想模型：无摩擦、无延迟（历史默认行为）。
+    #[must_use]
+    pub fn ideal_params() -> PlantParams {
+        PlantParams::gm3508()
+    }
+
+    /// 台架量级模型：静 / 库仑摩擦 + 2 拍延迟（整定默认）。
+    #[must_use]
+    pub fn bench_params() -> PlantParams {
+        PlantParams::gm3508_bench()
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn ideal_model_has_no_friction_or_delay() {
+            let params = ideal_params();
+            assert!(params.static_raw.abs() < 1e-6);
+            assert!(params.coulomb_raw.abs() < 1e-6);
+            assert_eq!(params.delay_ticks, 0);
+        }
+
+        #[test]
+        fn bench_model_has_the_measured_friction_and_delay() {
+            let params = bench_params();
+            assert!((400.0..=600.0).contains(&params.static_raw), "{}", params.static_raw);
+            assert!(params.coulomb_raw > 0.0);
+            assert_eq!(params.delay_ticks, 2);
+            // 台架模型只在摩擦 / 延迟上更真实，增益与上限应保持一致。
+            let ideal = ideal_params();
+            assert!((params.gain_rpm_per_raw - ideal.gain_rpm_per_raw).abs() < 1e-9);
+            assert!((params.max_rpm - ideal.max_rpm).abs() < 1e-6);
+        }
+    }
+}
+
+pub use dc_motor::{PlantMotor, PlantParams};
+pub use gm3508::{bench_params, ideal_params};