122 lines
4.7 KiB
Rust
122 lines
4.7 KiB
Rust
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||
//!
|
||
//! ```sh
|
||
//! socketcan-spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||
//! cargo run --release -p chassis-bench --bin socketcan-spin -- can0 1 1000 10
|
||
//! ```
|
||
//!
|
||
//! 关键点在 `spin_one`：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（`dji-esc-socketcan`）在此汇合。
|
||
//!
|
||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||
|
||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||
|
||
#[cfg(target_os = "linux")]
|
||
use std::time::Duration;
|
||
|
||
#[cfg(target_os = "linux")]
|
||
use dji_esc::{BusOptions, EscBus, EscConfig};
|
||
#[cfg(target_os = "linux")]
|
||
use dji_esc_socketcan::open_link;
|
||
#[cfg(target_os = "linux")]
|
||
use motor_core::Motor;
|
||
#[cfg(target_os = "linux")]
|
||
use tokio::time::{MissedTickBehavior, interval};
|
||
|
||
#[cfg(target_os = "linux")]
|
||
#[tokio::main]
|
||
async fn main() {
|
||
let mut args = std::env::args().skip(1);
|
||
let interface = args.next().unwrap_or_else(|| "can0".into());
|
||
let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
|
||
let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
|
||
let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
|
||
|
||
println!("=== 单电机抽象验证 ===");
|
||
println!(
|
||
"接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s"
|
||
);
|
||
|
||
let link = open_link(&interface).unwrap_or_else(|e| {
|
||
eprintln!(
|
||
"打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）"
|
||
);
|
||
std::process::exit(1);
|
||
});
|
||
let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
|
||
println!(
|
||
"电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}",
|
||
config.model, config.gear_ratio, config.current_limit
|
||
);
|
||
|
||
let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法");
|
||
let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
|
||
|
||
spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
|
||
|
||
bus.shutdown().await;
|
||
println!("已退出");
|
||
}
|
||
|
||
/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
|
||
/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
|
||
///
|
||
/// 注意**命令租约**：`BusOptions::command_timeout` 默认 50ms，超过该时长未重发
|
||
/// 命令，驱动会强制 0 电流。因此这里用 10ms 的租约拍持续重发同一个设定值
|
||
/// （真实底盘的控制循环每 1ms 重发一次）。
|
||
#[cfg(target_os = "linux")]
|
||
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||
const LEASE_PERIOD: Duration = Duration::from_millis(10);
|
||
const REPORT_PERIOD: Duration = Duration::from_millis(100);
|
||
|
||
motor.set_velocity(target_rpm);
|
||
println!("已下发目标转速，开始观测（100ms 周期）……\n");
|
||
|
||
let mut lease = interval(LEASE_PERIOD);
|
||
lease.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||
let mut report = interval(REPORT_PERIOD);
|
||
report.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||
let deadline = tokio::time::Instant::now() + duration;
|
||
|
||
loop {
|
||
tokio::select! {
|
||
_ = tokio::signal::ctrl_c() => {
|
||
println!("\n收到 ctrl-c");
|
||
break;
|
||
}
|
||
_ = lease.tick() => motor.set_velocity(target_rpm), // 续租
|
||
_ = report.tick() => {
|
||
let st = motor.state();
|
||
if st.has_feedback() {
|
||
println!(
|
||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||
st.rotor_speed_rpm,
|
||
st.effort_raw,
|
||
st.temperature_c,
|
||
st.rotor_revolutions,
|
||
st.feedback_count
|
||
);
|
||
} else {
|
||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||
}
|
||
}
|
||
}
|
||
if tokio::time::Instant::now() >= deadline {
|
||
break;
|
||
}
|
||
}
|
||
|
||
motor.disable();
|
||
// 让 0 电流帧至少发出一拍再关总线。
|
||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||
println!("已停机（0 电流）");
|
||
}
|
||
|
||
#[cfg(not(target_os = "linux"))]
|
||
fn main() {
|
||
eprintln!("spin 仅支持 Linux（需要 SocketCAN）");
|
||
}
|