refactor: 重构架构并整理仓库

crates/motor-control/Cargo.toml

@@ -0,0 +1,17 @@
+[package]
+name = "motor-control"
+description = "Motor velocity, position, cascade, and feedforward control"
+version.workspace = true
+edition.workspace = true
+rust-version.workspace = true
+license.workspace = true
+
+[lints]
+workspace = true
+
+[dependencies]
+pid = { workspace = true }
+motor = { workspace = true }
+
+[dev-dependencies]
+approx = { workspace = true }

crates/motor-control/src/lib.rs

@@ -0,0 +1,798 @@
+#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
+
+pub mod feedforward {
+    //! 前馈补偿（ARCHITECTURE.md §5.5）。
+    //!
+    //! 速度环的前馈是**比例前馈**：稳态下所需控制量与目标转速近似成正比，
+    //! 因此 `u_ff = k_ff · setpoint` 可以直接把积分项从"慢慢顶"变成"一开始就到位"。
+    //!
+    //! # 为什么前馈在空载台架上必须为 0
+    //!
+    //! `k_ff ≈ 1/K`，其中 `K` 是被控对象的稳态增益（GM3508 空载实测
+    //! `K ≈ 0.57 rpm/raw` → `k_ff ≈ 1.77`）。空载时维持转速只需要克服摩擦的极小
+    //! 电流，任何接近 1.77 的前馈都会把轮子直接推到最高转速（见 `docs/chassis.md` §7.2）。
+
+    /// 比例前馈：`输出 = k_ff · setpoint`。
+    #[derive(Debug, Clone, Copy, PartialEq, Default)]
+    pub struct ProportionalFeedforward {
+        /// 前馈系数（控制量 / 目标量）。
+        pub k_ff: f32,
+    }
+
+    impl ProportionalFeedforward {
+        /// 构造。
+        #[must_use]
+        pub const fn new(k_ff: f32) -> Self {
+            Self { k_ff }
+        }
+
+        /// 由被控对象稳态增益换算：`k_ff = 1 / K`。
+        ///
+        /// `K` 非有限或 ≤ 0 时返回 0（宁可不前馈，也不要一个发散的系数）。
+        #[must_use]
+        pub fn from_plant_gain(plant_gain: f32) -> Self {
+            if plant_gain.is_finite() && plant_gain > 0.0 {
+                Self { k_ff: 1.0 / plant_gain }
+            } else {
+                Self { k_ff: 0.0 }
+            }
+        }
+
+        /// 计算前馈量。
+        #[must_use]
+        pub fn compute(&self, setpoint: f32) -> f32 {
+            if setpoint.is_finite() { self.k_ff * setpoint } else { 0.0 }
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn inverse_plant_gain_matches_the_documented_bench_feedforward() {
+            // GM3508 空载 K ≈ 0.5648 rpm/raw → k_ff ≈ 1.77 raw/rpm
+            let feedforward = ProportionalFeedforward::from_plant_gain(9254.0 / 16384.0);
+            assert!((feedforward.k_ff - 1.77).abs() < 0.02, "{}", feedforward.k_ff);
+        }
+
+        #[test]
+        fn degenerate_plant_gain_yields_no_feedforward() {
+            assert!((ProportionalFeedforward::from_plant_gain(0.0).k_ff).abs() < 1e-6);
+            assert!((ProportionalFeedforward::from_plant_gain(-1.0).k_ff).abs() < 1e-6);
+            assert!((ProportionalFeedforward::from_plant_gain(f32::NAN).k_ff).abs() < 1e-6);
+        }
+
+        #[test]
+        fn compute_is_linear_and_safe() {
+            let feedforward = ProportionalFeedforward::new(2.0);
+            assert!((feedforward.compute(3.0) - 6.0).abs() < 1e-6);
+            assert!((feedforward.compute(f32::INFINITY)).abs() < 1e-6);
+            assert!((ProportionalFeedforward::default().compute(100.0)).abs() < 1e-6);
+        }
+    }
+}
+
+pub use feedforward::*;
+
+pub mod plant {
+    //! 被控对象抽象：让**仿真与实机共用同一套 PID**（ARCHITECTURE.md §5.5）。
+    //!
+    //! 这是整定能否迁移到实机的关键 seam：整定器只认识 [`ControlPlant`]，
+    //! 因此"在仿真里搜出来的增益"与"在台架上验出来的增益"评估的是同一条接口。
+    //!
+    //! 实现方：
+    //! - 仿真：`sim-harness::SimPlant`（浮点积分，无 IO，可并行上千次）；
+    //! - 实机/台架：`bench` 的台架采样（每次试验 = 真机跑一条场景）。
+
+    /// 单输入单输出的被控对象。
+    ///
+    /// 约定：`step` 推进 `dt` 秒并返回**本次的测量输出**；输入是控制量
+    /// （`motor-control` 里即 effort raw）。实现必须保证 `dt <= 0` 时不推进状态。
+    pub trait ControlPlant {
+        /// 推进一拍的误差类型（平台相关实现可以自定义）。
+        type Error;
+
+        /// 推进一步，返回测量输出。
+        ///
+        /// # Errors
+        ///
+        /// 数值发散或输入非法时返回错误，由调用方折算成硬约束罚值。
+        fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error>;
+
+        /// 复位到初始状态（两次试验之间必须调用，否则结果依赖历史）。
+        fn reset(&mut self);
+
+        /// 人类可读描述（写进报告，便于复现实验条件）。
+        fn describe(&self) -> String;
+    }
+
+    /// 无错误的被控对象（纯代数模型 / 已在内部处理发散的实现）用它的适配器。
+    #[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
+    pub struct Infallible;
+
+    impl core::fmt::Display for Infallible {
+        fn fmt(&self, _f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+            Ok(())
+        }
+    }
+
+    impl core::error::Error for Infallible {}
+
+    /// 一阶惯性环节：`y' = (k·u − y) / tau`，显式欧拉。
+    ///
+    /// 这是最小可用的 [`ControlPlant`] 实现，用来给控制律 / 整定器做单元测试；
+    /// 带摩擦与延迟的完整电机模型在 `sim-harness`。
+    #[derive(Debug, Clone, Copy, PartialEq)]
+    pub struct FirstOrderPlant {
+        /// 稳态增益。
+        pub gain: f32,
+        /// 时间常数（秒）。
+        pub tau_s: f32,
+        /// 当前输出。
+        pub output: f32,
+    }
+
+    impl FirstOrderPlant {
+        /// 构造（`tau_s` 必须为正，否则退化为直通）。
+        #[must_use]
+        pub fn new(gain: f32, tau_s: f32) -> Self {
+            Self { gain, tau_s: if tau_s > 0.0 { tau_s } else { f32::EPSILON }, output: 0.0 }
+        }
+    }
+
+    impl ControlPlant for FirstOrderPlant {
+        type Error = Infallible;
+
+        fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
+            if dt > 0.0 && input.is_finite() {
+                let alpha = (dt / self.tau_s).clamp(0.0, 1.0);
+                let target = self.gain * input;
+                self.output += alpha * (target - self.output);
+            }
+            Ok(self.output)
+        }
+
+        fn reset(&mut self) {
+            self.output = 0.0;
+        }
+
+        fn describe(&self) -> String {
+            format!("first-order(gain={}, tau={}s)", self.gain, self.tau_s)
+        }
+    }
+
+    /// 标准二阶系统：`G(s) = gain·wn² / (s² + 2ζ·wn·s + wn²)`，显式欧拉。
+    ///
+    /// 用作"整定算法是否真的收敛"的试金石——ARCHITECTURE.md §5.5 的验收标准就要求
+    /// 继电整定在标准二阶系统上收敛。
+    ///
+    /// # 相频特性（做继电实验时必须知道）
+    ///
+    /// `∠G(jω) = −atan2(2ζ·wn·ω, wn² − ω²)`：在 `ω = wn` 处只有 **−90°**，
+    /// 随 `ω → ∞` **渐近**趋于 −180°，**不存在有限的 −180° 穿越点**。
+    /// 因此纯二阶对象在继电反馈下的极限环由**滞环宽度**决定（幅值 ≈ 滞环量级、
+    /// 周期远小于 `2π/wn`），而不是由 `Tu = 2π/wn` 决定。
+    /// 真实电机带延迟（`sim-harness` 的模型有 2 拍命令延迟 + 摩擦），相频会真正穿过
+    /// −180°，那时的 `Tu` 才是有物理意义的临界周期。
+    #[derive(Debug, Clone, Copy, PartialEq)]
+    pub struct SecondOrderPlant {
+        /// 自然频率（rad/s）。
+        pub wn: f32,
+        /// 阻尼比。
+        pub zeta: f32,
+        /// 直流增益。
+        pub gain: f32,
+        /// 位置状态。
+        pub position: f32,
+        /// 速度状态。
+        pub velocity: f32,
+    }
+
+    impl SecondOrderPlant {
+        /// 构造（`wn > 0`；`zeta` 取绝对值）。
+        #[must_use]
+        pub fn new(wn: f32, zeta: f32, gain: f32) -> Self {
+            Self {
+                wn: if wn > 0.0 { wn } else { f32::EPSILON },
+                zeta: zeta.abs(),
+                gain,
+                position: 0.0,
+                velocity: 0.0,
+            }
+        }
+    }
+
+    impl ControlPlant for SecondOrderPlant {
+        type Error = Infallible;
+
+        fn step(&mut self, input: f32, dt: f32) -> Result<f32, Self::Error> {
+            if dt > 0.0 && input.is_finite() {
+                let accel = self.gain * self.wn * self.wn * input
+                    - 2.0 * self.zeta * self.wn * self.velocity
+                    - self.wn * self.wn * self.position;
+                self.velocity += accel * dt;
+                self.position += self.velocity * dt;
+            }
+            Ok(self.position)
+        }
+
+        fn reset(&mut self) {
+            self.position = 0.0;
+            self.velocity = 0.0;
+        }
+
+        fn describe(&self) -> String {
+            format!("second-order(wn={} rad/s, zeta={}, gain={})", self.wn, self.zeta, self.gain)
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn second_order_settles_at_dc_gain() {
+            let mut plant = SecondOrderPlant::new(20.0, 0.7, 2.0);
+            for _ in 0..20_000 {
+                plant.step(1.0, 0.0001).unwrap();
+            }
+            assert!((plant.position - 2.0).abs() < 5e-2, "{}", plant.position);
+            plant.reset();
+            assert!((plant.position).abs() < 1e-6);
+            assert!(plant.describe().contains("second-order"));
+        }
+
+        #[test]
+        fn second_order_ignores_non_positive_dt() {
+            let mut plant = SecondOrderPlant::new(20.0, 0.5, 1.0);
+            assert!((plant.step(1.0, 0.0).unwrap()).abs() < 1e-6);
+            assert!((plant.velocity).abs() < 1e-6);
+        }
+
+        #[test]
+        fn first_order_converges_to_gain_times_input() {
+            let mut plant = FirstOrderPlant::new(2.0, 0.1);
+            for _ in 0..2000 {
+                plant.step(1.0, 0.001).unwrap();
+            }
+            assert!((plant.output - 2.0).abs() < 1e-3, "{}", plant.output);
+        }
+
+        #[test]
+        fn non_positive_dt_does_not_advance() {
+            let mut plant = FirstOrderPlant::new(1.0, 0.1);
+            assert!((plant.step(5.0, 0.0).unwrap()).abs() < 1e-6);
+            assert!((plant.step(5.0, -1.0).unwrap()).abs() < 1e-6);
+            assert!((plant.step(f32::NAN, 0.001).unwrap()).abs() < 1e-6);
+        }
+
+        #[test]
+        fn reset_clears_history() {
+            let mut plant = FirstOrderPlant::new(1.0, 0.05);
+            for _ in 0..100 {
+                plant.step(1.0, 0.001).unwrap();
+            }
+            assert!(plant.output > 0.0);
+            plant.reset();
+            assert!((plant.output).abs() < 1e-6);
+            assert!(plant.describe().contains("first-order"));
+        }
+    }
+}
+
+pub use plant::*;
+
+pub mod position {
+    //! 位置环控制器：单环（直出）或级联（位置→限速→速度环）。
+
+    use crate::velocity::VelocityController;
+    use pid::Pid;
+
+    /// 位置环拓扑。
+    #[derive(Debug, Clone)]
+    pub enum PositionLoop {
+        /// 单环：位置误差 → effort_raw（适合电压硬闭环 / 刚性云台）。
+        Direct(Pid),
+        /// 级联：位置 → 目标转速 → 速度环 → effort_raw。
+        Cascade {
+            /// 外环位置 PID（输出限幅 = 最大 rpm）。
+            pos: Pid,
+            /// 内环速度控制器（输出限幅 = 最大 effort）。
+            vel: VelocityController,
+        },
+    }
+
+    /// 位置环封装。输入为目标 / 实测多圈位置（圈），输出为 effort_raw。
+    #[derive(Debug, Clone)]
+    pub struct PositionController {
+        inner: PositionLoop,
+    }
+
+    impl PositionController {
+        /// 单环位置控制器。
+        #[must_use]
+        pub fn direct(pid: Pid) -> Self {
+            Self { inner: PositionLoop::Direct(pid) }
+        }
+
+        /// 级联位置控制器（限速）。
+        #[must_use]
+        pub fn cascade(pos: Pid, vel: VelocityController) -> Self {
+            Self { inner: PositionLoop::Cascade { pos, vel } }
+        }
+
+        /// 单步更新，返回 effort_raw（已含限幅）。
+        ///
+        /// `measured_rpm` 仅级联内环使用；单环忽略该参数。
+        pub fn update(
+            &mut self,
+            target_rev: f32,
+            measured_rev: f32,
+            measured_rpm: f32,
+            dt: f32,
+        ) -> f32 {
+            match &mut self.inner {
+                PositionLoop::Direct(pid) => pid.update(target_rev, measured_rev, dt),
+                PositionLoop::Cascade { pos, vel } => {
+                    let vel_target = pos.update(target_rev, measured_rev, dt);
+                    vel.update(vel_target, measured_rpm, dt)
+                }
+            }
+        }
+
+        /// 清空内部状态（切换模式 / 重新使能时调用）。
+        pub fn reset(&mut self) {
+            match &mut self.inner {
+                PositionLoop::Direct(pid) => pid.reset(),
+                PositionLoop::Cascade { pos, vel } => {
+                    pos.reset();
+                    vel.reset();
+                }
+            }
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        const DT: f32 = 0.001;
+
+        #[test]
+        fn direct_outputs_pid() {
+            let mut pc = PositionController::direct(Pid::new(2.0, 0.0, 0.0, -100.0, 100.0));
+            let out = pc.update(1.0, 0.5, 0.0, DT);
+            assert!((out - 1.0).abs() < 1e-4, "out = {out}");
+        }
+
+        #[test]
+        fn cascade_limits_velocity_then_effort() {
+            // 外环 kp=10 → 误差 1 圈输出 10 rpm（限幅 5 rpm）；内环纯前馈 2 raw/rpm。
+            let pos = Pid::new(10.0, 0.0, 0.0, -5.0, 5.0);
+            let vel = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -1000.0, 1000.0), 2.0);
+            let mut pc = PositionController::cascade(pos, vel);
+            let out = pc.update(1.0, 0.0, 0.0, DT);
+            // 外环饱和到 5 rpm，内环 ff=2 → 10 raw。
+            assert!((out - 10.0).abs() < 1e-3, "out = {out}");
+        }
+
+        #[test]
+        fn reset_clears_state() {
+            let mut pc = PositionController::direct(Pid::new(0.0, 10.0, 0.0, -1e6, 1e6));
+            pc.update(1.0, 0.0, 0.0, DT);
+            pc.reset();
+            let out = pc.update(1.0, 0.0, 0.0, DT);
+            // 重置后只剩本步积分：10 * 1 * 0.001。
+            assert!((out - 0.01).abs() < 1e-5, "out = {out}");
+        }
+    }
+}
+
+pub use position::*;
+
+pub mod velocity {
+    //! 速度环控制器：PID + 速度前馈。
+
+    use pid::Pid;
+
+    /// 速度环增益组（可整定参数）。
+    ///
+    /// 单位：`kp` 为 raw / rpm，`ki` 为 raw / (rpm·s)，`kd` 为 raw / (rpm/s)，
+    /// `feedforward` 为 raw / rpm。整套增益是**纯数据**，便于整定工具在候选之间
+    /// 传递、落盘与热更新，而无需重建整个速度环。
+    #[derive(Debug, Clone, Copy, PartialEq)]
+    pub struct VelocityGains {
+        /// 比例增益（raw / rpm）。
+        pub kp: f32,
+        /// 积分增益（raw / (rpm·s)）。
+        pub ki: f32,
+        /// 微分增益（raw / (rpm/s)）。
+        pub kd: f32,
+        /// 速度前馈（raw / rpm）。
+        pub feedforward: f32,
+    }
+
+    impl VelocityGains {
+        /// 全零增益。
+        pub const ZERO: Self = Self { kp: 0.0, ki: 0.0, kd: 0.0, feedforward: 0.0 };
+
+        /// 由四个分量构造。
+        #[must_use]
+        pub const fn new(kp: f32, ki: f32, kd: f32, feedforward: f32) -> Self {
+            Self { kp, ki, kd, feedforward }
+        }
+
+        /// 所有增益是否有限（NaN / inf 不得进入 [`Pid::new`]，它会 panic）。
+        #[must_use]
+        pub fn is_finite(&self) -> bool {
+            self.kp.is_finite()
+                && self.ki.is_finite()
+                && self.kd.is_finite()
+                && self.feedforward.is_finite()
+        }
+    }
+
+    /// 速度环封装：在 [`Pid`] 之上叠加比例前馈。
+    ///
+    /// 输入为目标 / 实测转速（rpm，物理方向），输出为开环控制量
+    /// effort_raw（协议 raw：ESC=电流，GM6020=电压）。
+    /// 前馈系数 `feedforward` 的单位是 **raw / rpm**——稳态下所需控制量
+    /// 与转速近似成正比（GM3508 电流典型值约 9 raw/rpm，需按实车整定）。
+    ///
+    /// 由驱动层按控制节拍（如 C620 的 1kHz 发送周期）调用。
+    #[derive(Debug, Clone)]
+    pub struct VelocityController {
+        pid: Pid,
+        feedforward: f32,
+    }
+
+    impl VelocityController {
+        /// 创建速度环。`pid` 的输出限幅应设为该电机的 effort 限幅（raw）。
+        #[must_use]
+        pub fn new(pid: Pid, feedforward: f32) -> Self {
+            Self { pid, feedforward }
+        }
+
+        /// 由增益组与 effort 限幅直接创建（把 [`VelocityGains`] 展开成 [`Pid`]）。
+        ///
+        /// # Panics
+        ///
+        /// `output_limit_raw` 为 0，或增益含 NaN / inf 时 panic（同 [`Pid::new`]）。
+        #[must_use]
+        pub fn from_gains(output_limit_raw: i16, gains: VelocityGains) -> Self {
+            let limit = f32::from(output_limit_raw);
+            let pid = Pid::new(gains.kp, gains.ki, gains.kd, -limit, limit);
+            Self { pid, feedforward: gains.feedforward }
+        }
+
+        /// 当前增益组（含前馈），用于诊断与热更新前的读取。
+        #[must_use]
+        pub fn gains(&self) -> VelocityGains {
+            VelocityGains {
+                kp: self.pid.kp(),
+                ki: self.pid.ki(),
+                kd: self.pid.kd(),
+                feedforward: self.feedforward,
+            }
+        }
+
+        /// 单步更新，返回 effort_raw（已含 PID 输出限幅）。
+        pub fn update(&mut self, target_rpm: f32, measured_rpm: f32, dt: f32) -> f32 {
+            let pid_out = self.pid.update(target_rpm, measured_rpm, dt);
+            (pid_out + self.feedforward * target_rpm).clamp(
+                // 前馈可能把输出推出限幅之外，这里以 PID 限幅为准再夹一次。
+                self.pid.output_min(),
+                self.pid.output_max(),
+            )
+        }
+
+        /// 清空 PID 内部状态（切换模式 / 重新使能时调用）。
+        pub fn reset(&mut self) {
+            self.pid.reset();
+        }
+
+        /// 前馈系数（raw / rpm）。
+        #[must_use]
+        pub fn feedforward(&self) -> f32 {
+            self.feedforward
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+
+        #[test]
+        fn pure_feedforward_tracks_target() {
+            let mut vc = VelocityController::new(Pid::new(0.0, 0.0, 0.0, -16384.0, 16384.0), 9.0);
+            assert!((vc.update(1000.0, 0.0, 0.001) - 9000.0).abs() < 1e-3);
+            // 前馈 -18000 被限幅到 -16384。
+            assert!((vc.update(-2000.0, 0.0, 0.001) + 16384.0).abs() < 1e-3);
+        }
+
+        #[test]
+        fn feedforward_plus_pid_respects_limit() {
+            let mut vc = VelocityController::new(Pid::new(50.0, 0.0, 0.0, -12000.0, 12000.0), 9.0);
+            // 前馈 9000 + P 项 50*100 = 14000 → 限幅到 12000。
+            let out = vc.update(1000.0, 0.0, 0.001);
+            assert!((out - 12000.0).abs() < 1e-3, "out = {out}");
+        }
+
+        #[test]
+        fn zero_error_yields_pure_feedforward() {
+            // 误差为 0 时 PID 贡献为 0，输出只剩前馈。
+            let mut vc = VelocityController::new(Pid::new(10.0, 5.0, 0.0, -1e6, 1e6), 9.0);
+            let out = vc.update(1000.0, 1000.0, 0.001);
+            assert!((out - 9000.0).abs() < 1e-4, "out = {out}");
+        }
+
+        #[test]
+        fn reset_clears_pid_state() {
+            let mut vc = VelocityController::new(Pid::new(10.0, 100.0, 0.0, -1e6, 1e6), 0.0);
+            for _ in 0..10 {
+                vc.update(100.0, 0.0, 0.001);
+            }
+            vc.reset();
+            // 重置后首步输出 = 纯 P + 本步积分（ki*e*dt = 10）。
+            let out = vc.update(100.0, 0.0, 0.001);
+            assert!((out - 1010.0).abs() < 1e-3, "out = {out}");
+        }
+
+        #[test]
+        fn from_gains_round_trips_through_gains() {
+            let gains = VelocityGains::new(8.0, 2.0, 0.5, 1.77);
+            let mut from_data = VelocityController::from_gains(12_000, gains);
+            let mut by_hand =
+                VelocityController::new(Pid::new(8.0, 2.0, 0.5, -12_000.0, 12_000.0), 1.77);
+
+            assert_eq!(from_data.gains(), gains);
+            // 两条构造路径必须逐位一致。
+            for step in 0..16 {
+                let target = 1000.0 + step as f32;
+                let measured = 900.0 + step as f32 * 3.0;
+                assert_eq!(
+                    from_data.update(target, measured, 0.001).to_bits(),
+                    by_hand.update(target, measured, 0.001).to_bits()
+                );
+            }
+        }
+
+        #[test]
+        fn from_gains_clamps_to_output_limit() {
+            let mut vc =
+                VelocityController::from_gains(1000, VelocityGains::new(100.0, 0.0, 0.0, 0.0));
+            assert!((vc.update(1e5, 0.0, 0.001) - 1000.0).abs() < 1e-3);
+        }
+
+        #[test]
+        fn gains_finiteness_is_checkable() {
+            assert!(VelocityGains::new(1.0, 2.0, 3.0, 4.0).is_finite());
+            assert!(!VelocityGains::new(f32::NAN, 2.0, 3.0, 4.0).is_finite());
+            assert!(!VelocityGains::new(1.0, f32::INFINITY, 3.0, 4.0).is_finite());
+            assert_eq!(VelocityGains::ZERO, VelocityGains::new(0.0, 0.0, 0.0, 0.0));
+        }
+    }
+}
+
+pub use velocity::*;
+
+pub mod cascade {
+    //! 串级控制：位置环（外）+ 速度环（内）（ARCHITECTURE.md §5.5）。
+    //!
+    //! 外环输出的是**速度目标**而不是控制量，因此内环必须在每个节拍都跑一次——
+    //! 这也是云台（位置）与底盘（速度）共用的结构：底盘只用内环，云台两环都用。
+    //!
+    //! ```text
+    //! 位置目标 ──►[ 外环 PID ]──► 速度目标 ──►[ 内环 PID ]──► effort
+    //!                 ▲                            ▲
+    //!              位置反馈                     速度反馈
+    //! ```
+
+    use pid::Pid;
+
+    /// 位置-速度串级控制器。
+    #[derive(Debug, Clone)]
+    pub struct CascadeController {
+        /// 外环：位置 → 速度目标。
+        outer: Pid,
+        /// 内环：速度 → 控制量。
+        inner: Pid,
+    }
+
+    impl CascadeController {
+        /// 由两个已配置好的 PID 组装。
+        #[must_use]
+        pub const fn new(outer: Pid, inner: Pid) -> Self {
+            Self { outer, inner }
+        }
+
+        /// 单步更新，返回内环输出（控制量）。
+        ///
+        /// 非有限输入会被内环防御性忽略（见 [`Pid::update`]），因此本函数不会 panic。
+        pub fn compute(
+            &mut self,
+            position_setpoint: f32,
+            position_feedback: f32,
+            velocity_feedback: f32,
+            dt: f32,
+        ) -> f32 {
+            let velocity_setpoint = self.outer.update(position_setpoint, position_feedback, dt);
+            self.inner.update(velocity_setpoint, velocity_feedback, dt)
+        }
+
+        /// 外环当前建议的速度目标（诊断 / 限幅用）。
+        #[must_use]
+        pub fn last_velocity_setpoint(&self) -> f32 {
+            self.outer.last_output()
+        }
+
+        /// 清空两环的积分与微分记忆。
+        pub fn reset(&mut self) {
+            self.outer.reset();
+            self.inner.reset();
+        }
+    }
+
+    #[cfg(test)]
+    mod tests {
+        use super::*;
+        use crate::plant::{ControlPlant, FirstOrderPlant};
+
+        #[test]
+        fn cascade_drives_a_first_order_plant_to_the_position_setpoint() {
+            // 内环（速度）与对象同量纲简化：直接把外环输出当对象输入，
+            // 用真实的一阶对象验证"两环串联能收敛"。
+            let mut cascade = CascadeController::new(
+                Pid::new(4.0, 8.0, 0.0, -100.0, 100.0),
+                Pid::new(6.0, 20.0, 0.0, -100.0, 100.0),
+            );
+            let mut plant = FirstOrderPlant::new(1.0, 0.05);
+            let dt = 0.001;
+            let mut position = 0.0f32;
+            for _ in 0..4000 {
+                let effort = cascade.compute(1.0, position, 0.0, dt);
+                let velocity = plant.step(effort, dt).unwrap();
+                position += velocity * dt;
+            }
+            assert!((position - 1.0).abs() < 0.05, "position = {position}");
+            assert!(plant.describe().contains("first-order"));
+        }
+
+        #[test]
+        fn reset_clears_integral_windup() {
+            let mut cascade = CascadeController::new(
+                Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
+                Pid::new(1.0, 1.0, 0.0, -10.0, 10.0),
+            );
+            for _ in 0..100 {
+                cascade.compute(100.0, 0.0, 0.0, 0.001);
+            }
+            assert!(cascade.last_velocity_setpoint().abs() > 0.0);
+            cascade.reset();
+            assert!((cascade.last_velocity_setpoint()).abs() < 1e-6);
+        }
+    }
+}
+
+pub use cascade::*;
+
+/// Per-motor controller state used by [`MotorPolicy`].
+struct PolicyLoop {
+    current_limit: i16,
+    gains: VelocityGains,
+    velocity: VelocityController,
+    position: PositionController,
+    last_mode: motor::CommandMode,
+}
+
+impl PolicyLoop {
+    fn new(limit: i16, gains: VelocityGains) -> Self {
+        let bound = f32::from(limit);
+        Self {
+            current_limit: limit,
+            gains,
+            velocity: VelocityController::from_gains(limit, gains),
+            position: PositionController::direct(pid::Pid::new(
+                gains.kp, gains.ki, gains.kd, -bound, bound,
+            )),
+            last_mode: motor::CommandMode::Disabled,
+        }
+    }
+
+    fn reset(&mut self) {
+        self.velocity.reset();
+        self.position.reset();
+        self.last_mode = motor::CommandMode::Disabled;
+    }
+
+    fn effort(&mut self, sample: motor::PolicySample, dt: f32) -> i16 {
+        if !sample.online {
+            self.reset();
+            return 0;
+        }
+        if self.last_mode != sample.command.mode() {
+            self.reset();
+            self.last_mode = sample.command.mode();
+        }
+        match sample.command {
+            motor::Command::Disabled => {
+                self.reset();
+                0
+            }
+            motor::Command::EffortRaw(raw) => raw,
+            motor::Command::Velocity(target) => {
+                self.velocity.update(target, sample.state.rotor_speed_rpm, dt) as i16
+            }
+            motor::Command::Position(target) => self.position.update(
+                target,
+                sample.state.rotor_revolutions,
+                sample.state.rotor_speed_rpm,
+                dt,
+            ) as i16,
+        }
+    }
+}
+
+/// Concrete velocity/position policy for up to eight ESCs.
+pub struct MotorPolicy {
+    loops: [Option<PolicyLoop>; 8],
+}
+
+impl MotorPolicy {
+    /// Create an empty policy.
+    #[must_use]
+    pub fn new() -> Self {
+        Self { loops: std::array::from_fn(|_| None) }
+    }
+
+    /// Configure one ESC controller.
+    pub fn configure(&mut self, esc_id: u8, current_limit: i16, gains: VelocityGains) {
+        if let Some(slot) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
+            *slot = Some(PolicyLoop::new(current_limit, gains));
+        }
+    }
+
+    /// Read one controller's current gains.
+    #[must_use]
+    pub fn velocity_gains(&self, esc_id: u8) -> Option<VelocityGains> {
+        self.loops
+            .get(usize::from(esc_id.saturating_sub(1)))
+            .and_then(Option::as_ref)
+            .map(|loop_| loop_.gains)
+    }
+
+    /// Replace one controller's gains without changing its output limit.
+    pub fn set_velocity_gains(&mut self, esc_id: u8, gains: VelocityGains) {
+        if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
+            let limit = loop_.current_limit;
+            *loop_ = PolicyLoop::new(limit, gains);
+        }
+    }
+}
+
+impl Default for MotorPolicy {
+    fn default() -> Self {
+        Self::new()
+    }
+}
+
+impl motor::EffortPolicy for MotorPolicy {
+    fn step(&mut self, input: &motor::PolicyInput) -> [i16; 8] {
+        std::array::from_fn(|index| {
+            let sample = input.samples[index];
+            match self.loops[index].as_mut() {
+                Some(loop_) if sample.id == u8::try_from(index + 1).unwrap_or(0) => {
+                    loop_.effort(sample, input.dt)
+                }
+                _ => 0,
+            }
+        })
+    }
+
+    fn reset(&mut self, esc_id: u8) {
+        if let Some(Some(loop_)) = self.loops.get_mut(usize::from(esc_id.saturating_sub(1))) {
+            loop_.reset();
+        }
+    }
+}