feat: 初始化

2026-09-22 20:05:16 +08:00
commit 7d87987e41
54 changed files with 6446 additions and 0 deletions
@@ -0,0 +1,203 @@
+//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。
+
+use std::time::Duration;
+
+use mecanum::{Mecanum, Twist};
+use motor_core::{Disable, MotorFeedback, OutputShaft, VelocityDrive};
+use tokio::sync::watch;
+use tokio::task::JoinHandle;
+use tokio::time::{Instant, MissedTickBehavior, interval};
+use tracing::{info, warn};
+
+/// 底盘控制器。
+///
+/// 职责（1kHz 节拍）：
+/// 1. 读取最新 twist 指令，**指令超时则归零**；
+/// 2. 速度限幅（保持方向按比例收缩）；
+/// 3. **上电门控**：四台电机都收到过反馈才下发速度，否则全部停用；
+/// 4. 麦轮逆解 → 各轮目标转速（rpm，轮端）→ 经 [`OutputShaft`] 换算到转子。
+///
+/// 电机反馈离线安全在驱动层完成（强制 0 电流）。
+pub struct ChassisController<W> {
+    kinematics: Mecanum,
+    /// 顺序固定：FL、FR、RL、RR。轮端单位（经 `OutputShaft` 包装）。
+    wheels: [W; 4],
+    /// (max_vx, max_vy, max_omega)。
+    limits: (f32, f32, f32),
+    command_timeout: Duration,
+}
+
+impl<W> ChassisController<W>
+where
+    W: VelocityDrive + MotorFeedback + Disable + 'static,
+{
+    /// 组装控制器。`wheels` 顺序必须为 FL、FR、RL、RR。
+    #[must_use]
+    pub fn new(
+        kinematics: Mecanum,
+        wheels: [W; 4],
+        limits: (f32, f32, f32),
+        command_timeout: Duration,
+    ) -> Self {
+        Self { kinematics, wheels, limits, command_timeout }
+    }
+
+    /// 启动控制循环任务。
+    pub fn spawn(self, commands: watch::Receiver<Twist>, period: Duration) -> JoinHandle<()> {
+        tokio::spawn(self.run(commands, period))
+    }
+
+    async fn run(self, mut commands: watch::Receiver<Twist>, period: Duration) {
+        let mut ticker = interval(period);
+        ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
+        let mut latest = Twist::ZERO;
+        let mut last_command_at: Option<Instant> = None;
+        let mut ticks: u64 = 0;
+
+        loop {
+            ticker.tick().await;
+            let now = Instant::now();
+
+            if commands.has_changed().unwrap_or(false) {
+                latest = *commands.borrow_and_update();
+                last_command_at = Some(now);
+            }
+            let timed_out = match last_command_at {
+                Some(t) => now.duration_since(t) > self.command_timeout,
+                // 尚未收到任何指令：保持静止。
+                None => true,
+            };
+            let twist = if timed_out {
+                Twist::ZERO
+            } else {
+                latest.clamped(self.limits.0, self.limits.1, self.limits.2)
+            };
+
+            let all_seen_feedback = self.wheels.iter().all(|m| m.state().has_feedback());
+            if all_seen_feedback {
+                let wheel_rpms = self.kinematics.wheel_rpms(&twist);
+                for (motor, wheel_rpm) in self.wheels.iter().zip(wheel_rpms) {
+                    motor.set_velocity(wheel_rpm);
+                }
+            } else {
+                for motor in &self.wheels {
+                    motor.disable();
+                }
+                if ticks.is_multiple_of(1000) {
+                    warn!("waiting for first feedback from all motors");
+                }
+            }
+
+            ticks += 1;
+            if ticks.is_multiple_of(1000) {
+                self.log_status();
+            }
+        }
+    }
+
+    fn log_status(&self) {
+        let (rotor_rpm, effort_raw): (Vec<f32>, Vec<i32>) = self
+            .wheels
+            .iter()
+            .map(|m| {
+                let st = m.state();
+                (st.rotor_speed_rpm, i32::from(st.effort_raw))
+            })
+            .unzip();
+        info!(?rotor_rpm, ?effort_raw, "chassis status");
+    }
+}
+
+/// 由 `EscMotor` 列表组装轮端包装（`gear_ratio` = 转子 : 输出轴）。
+#[must_use]
+pub fn wrap_wheels<T: Clone, const N: usize>(
+    motors: [T; N],
+    gear_ratios: [f32; N],
+) -> [OutputShaft<T>; N] {
+    let mut out = Vec::with_capacity(N);
+    let mut gears = gear_ratios.into_iter();
+    for m in motors {
+        let g = gears.next().unwrap_or(1.0);
+        out.push(OutputShaft::new(m, g));
+    }
+    out.try_into().unwrap_or_else(|_| unreachable!())
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use motor_core::{Command, MockMotor, MotorState};
+
+    const GEAR: f32 = 19.203;
+
+    fn mock_wheel(id: u8) -> MockMotor {
+        let m = MockMotor::new(id);
+        m.inject_state(MotorState { feedback_count: 1, ..MotorState::default() });
+        m
+    }
+
+    /// 证明 `ChassisController` 只依赖能力 trait：换 `MockMotor` 原样工作。
+    #[tokio::test(start_paused = true)]
+    async fn controller_drives_wheels_through_capability_traits() {
+        let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
+        let mocks = [mock_wheel(1), mock_wheel(2), mock_wheel(3), mock_wheel(4)];
+        let ctrl = ChassisController::new(
+            kinematics,
+            wrap_wheels(mocks.clone(), [GEAR; 4]),
+            (3.0, 3.0, 6.0),
+            Duration::from_millis(200),
+        );
+        let (tx, rx) = watch::channel(Twist::ZERO);
+        let handle = ctrl.spawn(rx, Duration::from_millis(1));
+
+        let twist = Twist { vx: 1.0, ..Twist::ZERO };
+        tx.send_replace(twist);
+        tokio::time::sleep(Duration::from_millis(5)).await;
+
+        let expected = kinematics.wheel_rpms(&twist);
+        for (i, mock) in mocks.iter().enumerate() {
+            match mock.last_command() {
+                Command::Velocity(rotor_rpm) => {
+                    let want = expected[i] * GEAR;
+                    assert!(
+                        (rotor_rpm - want).abs() < 1e-2,
+                        "wheel {i}: rotor_rpm {rotor_rpm} != {want}"
+                    );
+                }
+                ref cmd => panic!("wheel {i} expected Velocity, got {cmd:?}"),
+            }
+        }
+
+        handle.abort();
+    }
+
+    /// 反馈未到齐时必须全部停用，不得下发速度。
+    #[tokio::test(start_paused = true)]
+    async fn disables_wheels_until_all_feedback_seen() {
+        let kinematics = Mecanum::new(0.2, 0.25, 0.0762);
+        let ready = mock_wheel(1);
+        let missing = MockMotor::new(2); // 无反馈
+        let mocks_ready = [ready.clone(), mock_wheel(3), mock_wheel(4)];
+        let _ = mocks_ready;
+        let wheels = wrap_wheels(
+            [ready.clone(), missing.clone(), mock_wheel(3), mock_wheel(4)],
+            [GEAR; 4],
+        );
+        let ctrl = ChassisController::new(
+            kinematics,
+            wheels,
+            (3.0, 3.0, 6.0),
+            Duration::from_millis(200),
+        );
+        let (tx, rx) = watch::channel(Twist::ZERO);
+        let handle = ctrl.spawn(rx, Duration::from_millis(1));
+
+        tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+        tokio::time::sleep(Duration::from_millis(5)).await;
+
+        assert_eq!(ready.last_command(), Command::Disabled);
+        assert_eq!(missing.last_command(), Command::Disabled);
+
+        handle.abort();
+    }
+}