linux_sentry_chassis/crates/chassisd/src/controller.rs

//! 底盘控制循环：麦轮逆解 → 四电机速度指令，含安全逻辑。

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();
    }
}