1173 lines
35 KiB
Markdown
1173 lines
35 KiB
Markdown
# RoboMaster CAN 控制系统架构文档
|
||
|
||
**版本**: 1.0
|
||
**目标平台**: Allwinner T153 (Cortex-A7 @ 1.6GHz, Linux)
|
||
**控制频率**: 500Hz
|
||
**文档目的**: 指导执行 Agent 完成项目拆分、结构整理与模块设计
|
||
|
||
---
|
||
|
||
## 目录
|
||
|
||
1. [系统概述](#1-系统概述)
|
||
2. [硬件拓扑](#2-硬件拓扑)
|
||
3. [架构原则](#3-架构原则)
|
||
4. [Crate 拆分](#4-crate-拆分)
|
||
5. [各 Crate 详细设计](#5-各-crate-详细设计)
|
||
6. [数据流与任务模型](#6-数据流与任务模型)
|
||
7. [实时性设计](#7-实时性设计)
|
||
8. [仿真与自动整定](#8-仿真与自动整定)
|
||
9. [测试策略](#9-测试策略)
|
||
10. [部署与配置](#10-部署与配置)
|
||
11. [执行 Agent 工作流](#11-执行-agent-工作流)
|
||
12. [附录](#12-附录)
|
||
|
||
---
|
||
|
||
## 1. 系统概述
|
||
|
||
### 1.1 目标
|
||
|
||
构建一个运行在 T153 Linux 平台上的 RoboMaster 机器人控制系统，实现：
|
||
|
||
- 对 **9 个 CAN 电机**（5 个原生 CAN + 4 个 SLCAN）的 500Hz 闭环控制
|
||
- 对 **delta10 IMU** 的姿态数据采集（通过 CAN 连接）
|
||
- 对机架不同部分的**完整功能抽象**（云台、发射机构、底盘）
|
||
- 支持**仿真环境下的自动 PID 整定**，并将参数迁移到实机
|
||
- 保证每一层的**可维护性、可验证性、可移植性**
|
||
|
||
### 1.2 关键约束
|
||
|
||
| 约束 | 值 | 影响 |
|
||
|------|-----|------|
|
||
| 控制频率 | 500Hz (2ms) | 任务预算分配、调度策略 |
|
||
| 平台 | T153 Cortex-A7 Linux | 非硬实时，需 SCHED_FIFO |
|
||
| CAN 接口 | 2 原生 + 1 SLCAN | 三种不同的访问路径 |
|
||
| IMU 接口 | CAN | 与电机共享 can0 总线 |
|
||
| 仿真目标 | 纯算法验证 | 不需要全系统数字孪生 |
|
||
|
||
---
|
||
|
||
## 2. 硬件拓扑
|
||
|
||
### 2.1 物理连接图
|
||
|
||
```
|
||
┌─────────────────────────────────────────────────────────────────┐
|
||
│ T153 (Linux, Cortex-A7) │
|
||
│ │
|
||
│ ┌──────────────┐ ┌──────────────┐ ┌──────────────────┐ │
|
||
│ │ CAN0 (原生) │ │ CAN1 (原生) │ │ USB CDC │ │
|
||
│ │ 1Mbps │ │ 1Mbps │ │ /dev/ttyACM0 │ │
|
||
│ └──────┬───────┘ └──────┬───────┘ └────────┬─────────┘ │
|
||
└─────────┼──────────────────┼────────────────────┼──────────────┘
|
||
│ │ │
|
||
│ │ │ (SLCAN 协议)
|
||
│ │ ▼
|
||
│ │ ┌──────────────────┐
|
||
│ │ │ candlelite2 │
|
||
│ │ │ (slcan 固件) │
|
||
│ │ └────────┬─────────┘
|
||
│ │ │
|
||
▼ ▼ ▼
|
||
┌──────────────────┐ ┌──────────┐ ┌───────────────────────┐
|
||
│ can0 总线 │ │can1 总线 │ │ SLCAN CAN 总线 │
|
||
│ │ │ │ │ │
|
||
│ ├─ GM6020 (yaw) │ │ ├─ M2006 │ │ ├─ M3508 (底盘 FL) │
|
||
│ ├─ GM6020(pitch)│ │ │ (拨弹) │ │ ├─ M3508 (底盘 FR) │
|
||
│ ├─ M3508 (摩擦L)│ │ │ │ │ ├─ M3508 (底盘 RL) │
|
||
│ ├─ M3508 (摩擦R)│ │ │ │ │ └─ M3508 (底盘 RR) │
|
||
│ └─ delta10 IMU │ │ │ │ │
|
||
└──────────────────┘ └──────────┘ └───────────────────────┘
|
||
```
|
||
|
||
### 2.2 设备清单
|
||
|
||
| 设备 | 数量 | 总线 | 协议 | 控制帧 ID | 反馈帧 ID |
|
||
|------|------|------|------|-----------|-----------|
|
||
| GM6020 (yaw) | 1 | can0 | 标准帧 | 0x1FF 或 0x2FF | 0x205-0x208 |
|
||
| GM6020 (pitch) | 1 | can0 | 标准帧 | 0x1FF 或 0x2FF | 0x205-0x208 |
|
||
| M3508 (摩擦轮) | 2 | can0 | 标准帧 | 0x200 或 0x1FF | 0x201-0x204 |
|
||
| M2006 (拨弹轮) | 1 | can1 | 标准帧 | 0x200 或 0x1FF | 0x201-0x204 |
|
||
| M3508 (底盘) | 4 | SLCAN | 标准帧 | 0x200 | 0x201-0x204 |
|
||
| delta10 IMU | 1 | can0 | FDILINK/标准 | TBD | TBD |
|
||
|
||
**注意**：GM6020 的 ID 分配需在初始化时固定，避免与 M3508 冲突。建议 GM6020 使用 0x205-0x206（对应 ID 1-2），M3508 摩擦轮使用 0x201-0x202（对应 ID 1-2），预留 ID 3-4。
|
||
|
||
### 2.3 总线负载估算
|
||
|
||
| 总线 | 设备数 | 帧/秒（500Hz） | 1Mbps 负载 |
|
||
|------|--------|----------------|-----------|
|
||
| can0 | 4 电机 + delta10 | ~5000 | ~12% |
|
||
| can1 | 1 电机 | ~1000 | ~2% |
|
||
| SLCAN | 4 电机 | ~4000 | USB CDC 带宽充裕 |
|
||
|
||
---
|
||
|
||
## 3. 架构原则
|
||
|
||
### 3.1 分层原则
|
||
|
||
系统分为 **7 层**，每层只依赖其下层，不允许跨层依赖：
|
||
|
||
```
|
||
┌─────────────────────────────────────────┐
|
||
│ L7: rm-app 应用层（任务编排） │
|
||
├─────────────────────────────────────────┤
|
||
│ L6: rm-sim 仿真层（纯算法验证） │
|
||
├─────────────────────────────────────────┤
|
||
│ L5: rm-control 控制层（PID/自整定） │
|
||
├─────────────────────────────────────────┤
|
||
│ L4: rm-devices 设备抽象层（Motor） │
|
||
├─────────────────────────────────────────┤
|
||
│ L3: rm-drivers 驱动层（SocketCAN/SLCAN）│
|
||
├─────────────────────────────────────────┤
|
||
│ L2: rm-protocol 协议层（帧编解码） │
|
||
├─────────────────────────────────────────┤
|
||
│ L1: rm-hal 硬件抽象层（trait） │
|
||
└─────────────────────────────────────────┘
|
||
```
|
||
|
||
**依赖方向**：L7 → L6 → L5 → L4 → L3 → L2 → L1。不允许反向依赖，不允许跳层依赖（如 L7 直接使用 L2）。
|
||
|
||
### 3.2 Rust 特性使用原则
|
||
|
||
| 特性 | 使用场景 | 示例 |
|
||
|------|---------|------|
|
||
| `const` 泛型 | 编译期 ID/通道校验 | `Gm6020Id<const ID: u8>` |
|
||
| `newtype` | 类型安全的值封装 | `Current(i16)`、`Angle(f32)` |
|
||
| `async fn in trait` | 异步接口抽象 | `CanBus::send` |
|
||
| `PhantomData` | 标记通道归属 | `SocketCanBus<const CH: u8>` |
|
||
| `enum` + `match` | 电机类型的 ad-hoc 多态 | `MotorKind` |
|
||
| `#[derive]` | 序列化/调试 | `zerocopy::FromBytes` |
|
||
| `no_std` | 纯算法层 | `rm-control`、`rm-protocol` |
|
||
|
||
### 3.3 单一职责原则
|
||
|
||
- **一个 crate 一个职责**：`rm-protocol` 只做编解码，不含 IO
|
||
- **一个模块一个概念**：`rm-devices/motor/gm6020.rs` 只处理 GM6020
|
||
- **一个函数一个动作**：`send_current` 只负责发送，不负责校验以外的逻辑
|
||
|
||
### 3.4 可移植性原则
|
||
|
||
- **硬件无关性**：`rm-control` 和 `rm-protocol` 不含任何平台相关代码
|
||
- **trait 边界**：所有硬件访问通过 `rm-hal` 的 trait
|
||
- **仿真对等**：`SimPlant` 和 `RealPlant` 实现同一 trait，可无缝切换
|
||
|
||
---
|
||
|
||
## 4. Crate 拆分
|
||
|
||
### 4.1 Workspace 结构
|
||
|
||
```
|
||
rm-can-robot/
|
||
├── Cargo.toml # workspace 根
|
||
├── rust-toolchain.toml # 工具链固定
|
||
├── .cargo/
|
||
│ └── config.toml # 交叉编译配置
|
||
├── crates/
|
||
│ ├── rm-hal/ # L1: 硬件抽象
|
||
│ ├── rm-protocol/ # L2: 协议编解码
|
||
│ ├── rm-drivers/ # L3: 驱动实现
|
||
│ ├── rm-devices/ # L4: 设备抽象
|
||
│ ├── rm-control/ # L5: 控制算法
|
||
│ ├── rm-sim/ # L6: 仿真
|
||
│ └── rm-app/ # L7: 应用
|
||
├── config/ # 运行时配置 (TOML)
|
||
│ ├── robot.toml # 机器人拓扑
|
||
│ ├── motors.toml # 电机参数
|
||
│ └── pid.toml # PID 参数
|
||
├── scripts/ # 部署与调试脚本
|
||
│ ├── setup_can.sh # CAN 接口初始化
|
||
│ ├── udev_rules.sh # 串口权限
|
||
│ └── deploy.sh # 交叉编译 + 部署
|
||
├── tests/ # 集成测试
|
||
│ ├── vcan_loopback.rs # 虚拟 CAN 回环
|
||
│ └── sim_pid_tune.rs # 仿真整定测试
|
||
└── docs/ # 架构文档
|
||
├── ARCHITECTURE.md # 本文档
|
||
├── PROTOCOL.md # CAN 协议说明
|
||
└── TUNING.md # 整定指南
|
||
```
|
||
|
||
### 4.2 Workspace Cargo.toml
|
||
|
||
```toml
|
||
[workspace]
|
||
resolver = "2"
|
||
members = [
|
||
"crates/rm-hal",
|
||
"crates/rm-protocol",
|
||
"crates/rm-drivers",
|
||
"crates/rm-devices",
|
||
"crates/rm-control",
|
||
"crates/rm-sim",
|
||
"crates/rm-app",
|
||
]
|
||
|
||
[workspace.package]
|
||
version = "0.1.0"
|
||
edition = "2021"
|
||
rust-version = "1.75"
|
||
license = "MIT"
|
||
|
||
[workspace.dependencies]
|
||
# 异步
|
||
tokio = { version = "1", features = ["rt-multi-thread", "time", "sync", "macros"] }
|
||
|
||
# CAN
|
||
socketcan = { version = "3.6", features = ["tokio"] }
|
||
slcan_fd = { version = "0.1", features = ["tokio"] }
|
||
tokio-serial = "5"
|
||
|
||
# 控制
|
||
discrete_pid = "0.2"
|
||
nalgebra = { version = "0.33", default-features = false, features = ["libm"] }
|
||
|
||
# 实时
|
||
libc = "0.2"
|
||
|
||
# 序列化
|
||
zerocopy = "0.8"
|
||
serde = { version = "1", features = ["derive"] }
|
||
toml = "0.8"
|
||
|
||
# 日志
|
||
tracing = "0.1"
|
||
tracing-subscriber = "0.3"
|
||
|
||
# 错误处理
|
||
thiserror = "1"
|
||
anyhow = "1"
|
||
|
||
# 测试
|
||
proptest = "1"
|
||
```
|
||
|
||
---
|
||
|
||
## 5. 各 Crate 详细设计
|
||
|
||
### 5.1 `rm-hal` — 硬件抽象层
|
||
|
||
**职责**：定义与硬件无关的 trait，作为所有硬件访问的抽象边界。
|
||
|
||
**依赖**：无（`no_std` 兼容）
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-hal/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── can.rs # CanBus trait + CanFrame
|
||
├── imu.rs # Imu trait
|
||
├── time.rs # 时间抽象
|
||
└── error.rs # 错误类型
|
||
```
|
||
|
||
**核心 trait**：
|
||
|
||
```rust
|
||
// can.rs
|
||
#[derive(Clone, Debug)]
|
||
pub struct CanFrame {
|
||
pub id: u32,
|
||
pub data: [u8; 8],
|
||
pub dlc: u8,
|
||
pub is_extended: bool,
|
||
}
|
||
|
||
pub trait CanBus {
|
||
type Error;
|
||
async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error>;
|
||
async fn recv(&mut self) -> Result<CanFrame, Self::Error>;
|
||
fn try_recv(&mut self) -> Result<Option<CanFrame>, Self::Error>;
|
||
}
|
||
|
||
// imu.rs
|
||
pub struct ImuData {
|
||
pub accel: [f32; 3],
|
||
pub gyro: [f32; 3],
|
||
pub mag: [f32; 3],
|
||
pub roll: f32,
|
||
pub pitch: f32,
|
||
pub yaw: f32,
|
||
pub timestamp_ns: u64,
|
||
}
|
||
|
||
pub trait Imu {
|
||
type Error;
|
||
async fn read(&mut self) -> Result<ImuData, Self::Error>;
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 所有 trait 不依赖任何具体硬件
|
||
- [ ] `no_std` 下编译通过（`cargo build --no-default-features`）
|
||
- [ ] 有完整的文档注释
|
||
|
||
---
|
||
|
||
### 5.2 `rm-protocol` — 协议编解码层
|
||
|
||
**职责**：CAN 帧的编解码，纯逻辑，无 IO。
|
||
|
||
**依赖**：`rm-hal`（仅用于 `CanFrame` 类型）、`zerocopy`
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-protocol/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── frame.rs # CanFrame 扩展
|
||
├── motor/
|
||
│ ├── mod.rs
|
||
│ ├── gm6020.rs # GM6020 编解码
|
||
│ ├── m3508.rs # M3508 编解码
|
||
│ ├── m2006.rs # M2006 编解码
|
||
│ └── common.rs # 电机反馈结构
|
||
├── delta10.rs # delta10 编解码
|
||
└── id.rs # CAN ID 编译期校验
|
||
```
|
||
|
||
**核心设计**：
|
||
|
||
```rust
|
||
// id.rs — 编译期 ID 校验
|
||
pub struct MotorId<const ID: u8>;
|
||
|
||
impl<const ID: u8> MotorId<ID> {
|
||
pub const NEW: Self = {
|
||
assert!(ID >= 1 && ID <= 8, "Motor ID must be 1-8");
|
||
Self
|
||
};
|
||
}
|
||
|
||
// motor/gm6020.rs
|
||
#[derive(Debug, Clone, Copy)]
|
||
pub struct Gm6020Feedback {
|
||
pub angle: f32, // 0-360 度
|
||
pub speed: f32, // rpm
|
||
pub current: i16, // 原始电流值
|
||
pub temperature: u8,
|
||
}
|
||
|
||
#[derive(Debug, Clone, Copy)]
|
||
pub struct Gm6020Command {
|
||
pub current: i16, // -30000 ~ 30000
|
||
}
|
||
|
||
impl Gm6020Command {
|
||
pub fn to_frame<const ID: u8>(&self, id: MotorId<ID>) -> CanFrame {
|
||
// 编码到 0x1FF 或 0x2FF 广播帧
|
||
}
|
||
}
|
||
|
||
pub fn decode_feedback(frame: &CanFrame) -> Option<(u8, Gm6020Feedback)> {
|
||
// 从 0x205-0x208 解码
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 所有电机的编解码有单元测试
|
||
- [ ] 使用 `proptest` 做模糊测试
|
||
- [ ] `no_std` 兼容
|
||
- [ ] 无任何 IO 操作
|
||
|
||
---
|
||
|
||
### 5.3 `rm-drivers` — 驱动层
|
||
|
||
**职责**：实现 `rm-hal` 的 trait，对接具体硬件。
|
||
|
||
**依赖**：`rm-hal`、`rm-protocol`、`socketcan`、`slcan_fd`、`tokio-serial`
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-drivers/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── socketcan.rs # SocketCAN 实现 (can0, can1)
|
||
├── slcan.rs # SLCAN 实现 (candlelite2)
|
||
├── delta10.rs # delta10 IMU 驱动
|
||
└── mock.rs # 测试用 mock（feature-gated）
|
||
```
|
||
|
||
**核心实现**：
|
||
|
||
```rust
|
||
// socketcan.rs
|
||
pub struct SocketCanBus {
|
||
socket: socketcan::tokio::CanSocket,
|
||
}
|
||
|
||
impl CanBus for SocketCanBus {
|
||
type Error = socketcan::Error;
|
||
|
||
async fn send(&mut self, frame: &CanFrame) -> Result<(), Self::Error> {
|
||
let sock_frame = to_socketcan_frame(frame);
|
||
self.socket.write_frame(&sock_frame).await
|
||
}
|
||
|
||
async fn recv(&mut self) -> Result<CanFrame, Self::Error> {
|
||
let f = self.socket.read_frame().await?;
|
||
Ok(from_socketcan_frame(f))
|
||
}
|
||
}
|
||
|
||
// slcan.rs
|
||
pub struct SlcanBus {
|
||
socket: slcan_fd::tokio::CanSocket<tokio_serial::SerialStream>,
|
||
}
|
||
|
||
impl CanBus for SlcanBus {
|
||
// 类似实现，但底层使用 SLCAN 协议
|
||
}
|
||
|
||
// delta10.rs
|
||
pub struct Delta10Driver<B: CanBus> {
|
||
bus: B,
|
||
can_id: u16,
|
||
last_data: Option<ImuData>,
|
||
}
|
||
|
||
impl<B: CanBus> Imu for Delta10Driver<B> {
|
||
async fn read(&mut self) -> Result<ImuData, Self::Error> {
|
||
// 等待 delta10 帧，解码 FDILINK/标准协议
|
||
}
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 在 T153 上能成功打开 `can0`、`can1`、SLCAN 串口
|
||
- [ ] 能用 `candump` 验证帧收发
|
||
- [ ] 有 `mock` feature 用于测试
|
||
|
||
---
|
||
|
||
### 5.4 `rm-devices` — 设备抽象层
|
||
|
||
**职责**：将协议层的数据封装为有状态的电机设备，提供统一的 `Motor` trait。
|
||
|
||
**依赖**：`rm-hal`、`rm-protocol`
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-devices/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── traits.rs # Motor trait
|
||
├── gm6020.rs
|
||
├── m3508.rs
|
||
├── m2006.rs
|
||
└── registry.rs # 电机注册表（管理所有电机）
|
||
```
|
||
|
||
**核心设计**：
|
||
|
||
```rust
|
||
// traits.rs
|
||
pub trait Motor {
|
||
type Feedback;
|
||
type Command;
|
||
|
||
fn id(&self) -> u8;
|
||
fn set_command(&mut self, cmd: Self::Command) -> Result<(), MotorError>;
|
||
fn feedback(&self) -> Option<&Self::Feedback>;
|
||
fn update_feedback(&mut self, frame: &CanFrame) -> Result<(), MotorError>;
|
||
fn is_online(&self, now: Instant) -> bool;
|
||
fn latency_hint(&self) -> Duration; // SLCAN 返回较大值
|
||
}
|
||
|
||
// registry.rs — 统一管理所有电机
|
||
pub struct MotorRegistry {
|
||
gm6020: [Gm6020; 2],
|
||
m3508_arm: [M3508; 2],
|
||
m3508_chassis: [M3508; 4],
|
||
m2006: M2006,
|
||
}
|
||
|
||
impl MotorRegistry {
|
||
pub fn update_from_frame(&mut self, bus: BusId, frame: &CanFrame) {
|
||
// 根据总线 ID 和帧 ID 分发到对应电机
|
||
}
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 每个电机类型有独立的单元测试
|
||
- [ ] `MotorRegistry` 能正确分发帧
|
||
- [ ] 在线检测逻辑正确（超时阈值可配置）
|
||
|
||
---
|
||
|
||
### 5.5 `rm-control` — 控制算法层
|
||
|
||
**职责**：PID 控制、串级控制、前馈补偿、自动整定。纯算法，无 IO。
|
||
|
||
**依赖**：`discrete_pid`、`nalgebra`（可选）
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-control/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── pid.rs # PID 封装（基于 discrete_pid）
|
||
├── cascade.rs # 串级控制
|
||
├── feedforward.rs # 前馈补偿
|
||
├── plant.rs # ControlPlant trait
|
||
└── tuner/
|
||
├── mod.rs
|
||
├── relay.rs # Relay feedback 整定
|
||
└── zn.rs # Ziegler-Nichols 公式
|
||
```
|
||
|
||
**核心设计**：
|
||
|
||
```rust
|
||
// plant.rs — 抽象被控对象，使仿真和实机共用 PID
|
||
pub trait ControlPlant {
|
||
fn step(&mut self, input: f32, dt: f32) -> f32; // 返回测量输出
|
||
fn reset(&mut self);
|
||
}
|
||
|
||
// pid.rs
|
||
pub struct PidController {
|
||
inner: discrete_pid::Pid<f32>,
|
||
output_limit: (f32, f32),
|
||
integral_limit: (f32, f32),
|
||
}
|
||
|
||
impl PidController {
|
||
pub fn compute(&mut self, setpoint: f32, measured: f32, dt: f32) -> f32 {
|
||
let raw = self.inner.compute(setpoint, measured, dt);
|
||
raw.clamp(self.output_limit.0, self.output_limit.1)
|
||
}
|
||
}
|
||
|
||
// cascade.rs — 位置-速度串级
|
||
pub struct CascadeController {
|
||
outer: PidController, // 位置环
|
||
inner: PidController, // 速度环
|
||
}
|
||
|
||
impl CascadeController {
|
||
pub fn compute(&mut self, pos_sp: f32, pos_fb: f32, vel_fb: f32, dt: f32) -> f32 {
|
||
let vel_sp = self.outer.compute(pos_sp, pos_fb, dt);
|
||
self.inner.compute(vel_sp, vel_fb, dt)
|
||
}
|
||
}
|
||
|
||
// tuner/relay.rs
|
||
pub struct RelayTuner {
|
||
amplitude: f32,
|
||
hysteresis: f32,
|
||
state: RelayState,
|
||
measurements: Vec<OscillationPoint>,
|
||
}
|
||
|
||
pub struct OscillationPoint {
|
||
pub peak: f32,
|
||
pub trough: f32,
|
||
pub period: f32,
|
||
}
|
||
|
||
impl RelayTuner {
|
||
pub fn step(&mut self, measured: f32, dt: f32) -> f32 {
|
||
// 返回 relay 输出，记录振荡点
|
||
}
|
||
|
||
pub fn result(&self) -> Option<TuneResult> {
|
||
// Ziegler-Nichols 计算 Kp, Ki, Kd
|
||
}
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] PID 在阶跃输入下无超调（可配置）
|
||
- [ ] 抗积分饱和逻辑正确
|
||
- [ ] Relay tuner 在标准二阶系统上能收敛
|
||
- [ ] `no_std` 兼容
|
||
|
||
---
|
||
|
||
### 5.6 `rm-sim` — 仿真层
|
||
|
||
**职责**：提供电机模型、总线模型、测试场景，用于算法验证和自动整定。
|
||
|
||
**依赖**：`rm-control`、`rm-protocol`
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-sim/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── lib.rs
|
||
├── motor_model/
|
||
│ ├── mod.rs
|
||
│ ├── dc_motor.rs # 通用直流电机模型
|
||
│ ├── gm6020.rs # GM6020 参数化
|
||
│ ├── m3508.rs # M3508 参数化
|
||
│ └── m2006.rs # M2006 参数化
|
||
├── bus_model.rs # CAN 总线模型（延迟/丢帧）
|
||
├── plant.rs # SimPlant 实现
|
||
├── scenario.rs # 测试场景
|
||
└── tuner_runner.rs # 自动整定流程
|
||
```
|
||
|
||
**核心设计**：
|
||
|
||
```rust
|
||
// motor_model/dc_motor.rs
|
||
pub struct DcMotorParams {
|
||
pub resistance: f32, // 欧姆
|
||
pub inductance: f32, // 亨利
|
||
pub torque_constant: f32, // N·m/A
|
||
pub back_emf_constant: f32, // V·s/rad
|
||
pub gear_ratio: f32,
|
||
pub rotor_inertia: f32, // kg·m²
|
||
pub current_loop_bw: f32, // 电流环带宽 Hz
|
||
}
|
||
|
||
pub struct DcMotorModel {
|
||
params: DcMotorParams,
|
||
state: MotorState,
|
||
}
|
||
|
||
impl DcMotorModel {
|
||
pub fn step(&mut self, current_cmd: f32, dt: f32) -> MotorOutput {
|
||
// 电流环一阶滞后 → 转矩 → 加速度 → 速度 → 位置
|
||
}
|
||
}
|
||
|
||
// plant.rs — 仿真 plant 实现 ControlPlant
|
||
pub struct SimPlant<M: MotorModel> {
|
||
model: M,
|
||
noise: Option<NoiseModel>, // 编码器噪声
|
||
}
|
||
|
||
impl<M: MotorModel> ControlPlant for SimPlant<M> {
|
||
fn step(&mut self, input: f32, dt: f32) -> f32 {
|
||
let out = self.model.step(input, dt);
|
||
self.noise.map(|n| n.apply(out.position)).unwrap_or(out.position)
|
||
}
|
||
fn reset(&mut self) { self.model.reset(); }
|
||
}
|
||
|
||
// tuner_runner.rs
|
||
pub async fn auto_tune<P: ControlPlant>(
|
||
plant: &mut P,
|
||
config: TuneConfig,
|
||
) -> TuneResult {
|
||
let mut tuner = RelayTuner::new(config.amplitude, config.hysteresis);
|
||
let mut t = 0.0;
|
||
while t < config.max_time {
|
||
let output = tuner.step(plant.step(tuner.last_output(), config.dt), config.dt);
|
||
// ...
|
||
t += config.dt;
|
||
}
|
||
tuner.result().expect("Tuning failed")
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 电机模型在阶跃输入下的响应符合物理直觉
|
||
- [ ] 仿真整定出的 PID 在实机上只需 ±20% 微调
|
||
- [ ] 支持批量扫描不同参数
|
||
|
||
---
|
||
|
||
### 5.7 `rm-app` — 应用层
|
||
|
||
**职责**：任务编排、配置加载、实时调度、主循环。
|
||
|
||
**依赖**：所有下层 crate
|
||
|
||
**文件结构**：
|
||
|
||
```
|
||
rm-app/
|
||
├── Cargo.toml
|
||
└── src/
|
||
├── main.rs
|
||
├── config.rs # TOML 配置加载
|
||
├── realtime.rs # SCHED_FIFO + mlockall
|
||
├── tasks/
|
||
│ ├── mod.rs
|
||
│ ├── gimbal.rs # 云台任务 (can0)
|
||
│ ├── shooter.rs # 发射机构任务 (can0 + can1)
|
||
│ └── chassis.rs # 底盘任务 (SLCAN)
|
||
├── messages.rs # 任务间通信消息
|
||
└── telemetry.rs # 诊断输出
|
||
```
|
||
|
||
**任务模型**：
|
||
|
||
```rust
|
||
// main.rs
|
||
#[tokio::main]
|
||
async fn main() -> anyhow::Result<()> {
|
||
// 1. 实时调度
|
||
realtime::enable(80)?;
|
||
|
||
// 2. 加载配置
|
||
let config = config::load("config/robot.toml")?;
|
||
|
||
// 3. 打开 CAN 总线
|
||
let can0 = SocketCanBus::open("can0")?;
|
||
let can1 = SocketCanBus::open("can1")?;
|
||
let slcan = SlcanBus::open("/dev/ttyACM0")?;
|
||
|
||
// 4. 启动任务
|
||
let gimbal = tokio::spawn(tasks::gimbal::run(can0, config.gimbal));
|
||
let shooter = tokio::spawn(tasks::shooter::run(can1, config.shooter));
|
||
let chassis = tokio::spawn(tasks::chassis::run(slcan, config.chassis));
|
||
|
||
// 5. 等待
|
||
tokio::try_join!(gimbal, shooter, chassis)?;
|
||
Ok(())
|
||
}
|
||
|
||
// tasks/gimbal.rs
|
||
pub async fn run(mut bus: SocketCanBus, config: GimbalConfig) -> anyhow::Result<()> {
|
||
let mut motors = GimbalMotors::new();
|
||
let mut pid_yaw = PidController::new(config.yaw_pid);
|
||
let mut pid_pitch = PidController::new(config.pitch_pid);
|
||
let mut imu = Delta10Driver::new(&mut bus, config.imu_can_id);
|
||
let mut ticker = tokio::time::interval(Duration::from_millis(2));
|
||
|
||
loop {
|
||
ticker.tick().await;
|
||
|
||
// 1. 非阻塞读取 delta10 帧
|
||
if let Some(data) = imu.try_read()? {
|
||
// 更新姿态
|
||
}
|
||
|
||
// 2. 读取电机反馈
|
||
while let Some(frame) = bus.try_recv()? {
|
||
motors.update_from_frame(&frame)?;
|
||
}
|
||
|
||
// 3. 计算控制
|
||
let yaw_out = pid_yaw.compute(setpoint.yaw, imu.yaw, 0.002);
|
||
let pitch_out = pid_pitch.compute(setpoint.pitch, imu.pitch, 0.002);
|
||
|
||
// 4. 发送控制帧
|
||
motors.send_commands(&mut bus, yaw_out, pitch_out).await?;
|
||
}
|
||
}
|
||
```
|
||
|
||
**验收标准**：
|
||
- [ ] 三个任务独立运行，互不阻塞
|
||
- [ ] 每个任务在自己的 2ms 周期内完成
|
||
- [ ] 配置可通过 TOML 热加载（或至少无需重新编译）
|
||
|
||
---
|
||
|
||
## 6. 数据流与任务模型
|
||
|
||
### 6.1 任务划分
|
||
|
||
| 任务 | 频率 | 总线 | 设备 | 输出 |
|
||
|------|------|------|------|------|
|
||
| `gimbal_task` | 500Hz | can0 | 2× GM6020 + delta10 | 云台电流 |
|
||
| `shooter_task` | 500Hz | can0 + can1 | 2× M3508 + 1× M2006 | 摩擦轮 + 拨弹电流 |
|
||
| `chassis_task` | 500Hz | SLCAN | 4× M3508 | 底盘电流 |
|
||
|
||
### 6.2 任务间通信
|
||
|
||
```
|
||
┌──────────────┐
|
||
│ 遥控器输入 │ (未来扩展)
|
||
└──────┬───────┘
|
||
│ watch<RemoteCommand>
|
||
▼
|
||
┌──────────────────────────────────────────┐
|
||
│ tokio channels │
|
||
│ ┌────────────┐ ┌────────────┐ │
|
||
│ │ gimbal_cmd │ │chassis_cmd │ │
|
||
│ └─────┬──────┘ └─────┬──────┘ │
|
||
└────────┼────────────────┼────────────────┘
|
||
│ │
|
||
┌────▼────┐ ┌────▼────┐
|
||
│ gimbal │ │ chassis │
|
||
│ task │ │ task │
|
||
└─────────┘ └─────────┘
|
||
```
|
||
|
||
**通信原语选择**：
|
||
- **遥控器输入 → 任务**：`tokio::sync::watch`（单生产者多消费者，只关心最新值）
|
||
- **任务 → 诊断输出**：`tokio::sync::mpsc`（有背压）
|
||
- **任务间同步**：`tokio::sync::Barrier`（如需要）
|
||
|
||
### 6.3 每个周期的执行顺序
|
||
|
||
以 `gimbal_task` 为例：
|
||
|
||
```
|
||
t=0ms:
|
||
1. ticker.tick() 返回
|
||
2. 非阻塞读取 delta10 帧 (try_recv)
|
||
3. 非阻塞读取所有 pending CAN 帧 (while try_recv)
|
||
4. 更新电机反馈
|
||
5. 计算 PID 输出
|
||
6. 发送控制帧 (await)
|
||
7. 记录诊断
|
||
|
||
t=2ms:
|
||
重复
|
||
```
|
||
|
||
**关键**：步骤 2-5 必须是**非阻塞**的，只有步骤 6 可以 `await`。这保证任务不会因为等待 CAN 帧而错过下一个 tick。
|
||
|
||
---
|
||
|
||
## 7. 实时性设计
|
||
|
||
### 7.1 调度策略
|
||
|
||
```rust
|
||
// realtime.rs
|
||
use libc::{sched_param, sched_setscheduler, SCHED_FIFO, mlockall, MCL_CURRENT, MCL_FUTURE};
|
||
|
||
pub fn enable(priority: i32) -> anyhow::Result<()> {
|
||
unsafe {
|
||
// 1. 锁定内存，避免页错误
|
||
if mlockall(MCL_CURRENT | MCL_FUTURE) != 0 {
|
||
return Err(std::io::Error::last_os_error().into());
|
||
}
|
||
|
||
// 2. 设置实时调度
|
||
let param = sched_param { sched_priority: priority };
|
||
if sched_setscheduler(0, SCHED_FIFO, ¶m) != 0 {
|
||
return Err(std::io::Error::last_os_error().into());
|
||
}
|
||
}
|
||
Ok(())
|
||
}
|
||
```
|
||
|
||
**优先级分配**：
|
||
- `gimbal_task`: 80（最高，因为云台抖动直接影响射击）
|
||
- `shooter_task`: 75
|
||
- `chassis_task`: 70
|
||
|
||
### 7.2 抖动测量
|
||
|
||
在每个任务的 tick 中记录实际唤醒时间：
|
||
|
||
```rust
|
||
let expected = start + Duration::from_millis(2 * tick_count);
|
||
let actual = Instant::now();
|
||
let jitter = actual.duration_since(expected);
|
||
telemetry::record_jitter(jitter);
|
||
```
|
||
|
||
**目标**：抖动 < 200μs（T153 标准 Linux 的典型值）。
|
||
|
||
### 7.3 降级策略
|
||
|
||
如果抖动超过阈值（如 500μs），采取：
|
||
|
||
1. **降低非关键任务频率**：chassis_task 降到 250Hz
|
||
2. **合并任务**：shooter 和 gimbal 合并
|
||
3. **异构核卸载**：将 CAN 收发卸载到 E907（远期）
|
||
|
||
---
|
||
|
||
## 8. 仿真与自动整定
|
||
|
||
### 8.1 仿真流程
|
||
|
||
```
|
||
┌─────────────────────────────────────────────────┐
|
||
│ 1. 定义电机参数 (config/motors.toml) │
|
||
│ ↓ │
|
||
│ 2. 构建 SimPlant<DcMotorModel> │
|
||
│ ↓ │
|
||
│ 3. 运行 auto_tune │
|
||
│ ↓ │
|
||
│ 4. 输出 PID 参数 → config/pid.toml │
|
||
│ ↓ │
|
||
│ 5. 在 SimPlant 上验证阶跃响应 │
|
||
│ ↓ │
|
||
│ 6. 部署到实机，微调 ±20% │
|
||
└─────────────────────────────────────────────────┘
|
||
```
|
||
|
||
### 8.2 电机参数配置
|
||
|
||
```toml
|
||
# config/motors.toml
|
||
[gm6020]
|
||
resistance = 0.35
|
||
inductance = 0.0002
|
||
torque_constant = 0.024
|
||
back_emf_constant = 0.024
|
||
gear_ratio = 1.0
|
||
rotor_inertia = 1.5e-5
|
||
current_loop_bw = 1000.0
|
||
|
||
[m3508]
|
||
resistance = 0.15
|
||
inductance = 0.0001
|
||
torque_constant = 0.015
|
||
back_emf_constant = 0.015
|
||
gear_ratio = 19.2
|
||
rotor_inertia = 2.0e-5
|
||
current_loop_bw = 800.0
|
||
|
||
[m2006]
|
||
# ...
|
||
```
|
||
|
||
### 8.3 自动整定示例
|
||
|
||
```rust
|
||
#[tokio::test]
|
||
async fn tune_m3508_speed_loop() {
|
||
let params = load_motor_params("config/motors.toml", "m3508");
|
||
let mut plant = SimPlant::new(DcMotorModel::new(params));
|
||
|
||
let config = TuneConfig {
|
||
amplitude: 5000.0, // 电流指令幅值
|
||
hysteresis: 100.0,
|
||
dt: 0.002,
|
||
max_time: 5.0,
|
||
};
|
||
|
||
let result = auto_tune(&mut plant, config).await;
|
||
|
||
// 验证整定结果
|
||
assert!(result.kp > 0.0);
|
||
assert!(result.ki > 0.0);
|
||
assert!(result.ku > 0.0);
|
||
assert!(result.tu > 0.0);
|
||
|
||
// 在仿真中验证阶跃响应
|
||
let mut pid = PidController::new(result.into());
|
||
let mut plant = SimPlant::new(DcMotorModel::new(params));
|
||
let mut max_overshoot = 0.0f32;
|
||
|
||
for _ in 0..500 { // 1 秒
|
||
let out = pid.compute(1000.0, plant.step(0.0, 0.002), 0.002);
|
||
let _ = plant.step(out, 0.002);
|
||
// 记录超调
|
||
}
|
||
|
||
assert!(max_overshoot < 0.1, "超调过大: {}", max_overshoot);
|
||
}
|
||
```
|
||
|
||
---
|
||
|
||
## 9. 测试策略
|
||
|
||
### 9.1 分层测试
|
||
|
||
| 层 | 测试类型 | 工具 |
|
||
|----|---------|------|
|
||
| `rm-protocol` | 单元测试 + 模糊测试 | `#[test]`, `proptest` |
|
||
| `rm-control` | 单元测试 + 仿真测试 | `#[test]`, `rm-sim` |
|
||
| `rm-devices` | 集成测试（mock CAN） | `mock` feature |
|
||
| `rm-drivers` | 硬件在环测试 | `vcan` 回环 |
|
||
| `rm-app` | 端到端测试 | `vcan` + 模拟电机 |
|
||
|
||
### 9.2 vcan 回环测试
|
||
|
||
```bash
|
||
# 创建虚拟 CAN 接口
|
||
sudo modprobe vcan
|
||
sudo ip link add dev vcan0 type vcan
|
||
sudo ip link set up vcan0
|
||
```
|
||
|
||
```rust
|
||
#[tokio::test]
|
||
async fn test_socketcan_loopback() {
|
||
let mut bus = SocketCanBus::open("vcan0").unwrap();
|
||
let frame = CanFrame { id: 0x1FF, data: [1,2,3,4,5,6,7,8], dlc: 8, is_extended: false };
|
||
bus.send(&frame).await.unwrap();
|
||
let received = bus.recv().await.unwrap();
|
||
assert_eq!(received.id, 0x1FF);
|
||
}
|
||
```
|
||
|
||
### 9.3 仿真回归测试
|
||
|
||
每次修改 `rm-control` 后，运行完整的仿真测试套件：
|
||
|
||
```bash
|
||
cargo test -p rm-sim --release
|
||
```
|
||
|
||
**回归基准**：阶跃响应、抗扰响应、整定收敛时间。任何指标退化 >5% 视为失败。
|
||
|
||
---
|
||
|
||
## 10. 部署与配置
|
||
|
||
### 10.1 CAN 接口初始化脚本
|
||
|
||
```bash
|
||
#!/bin/bash
|
||
# scripts/setup_can.sh
|
||
|
||
# 原生 CAN (T153)
|
||
ip link set can0 type can bitrate 1000000 restart-ms 100
|
||
ip link set can0 up
|
||
ip link set can1 type can bitrate 1000000 restart-ms 100
|
||
ip link set can1 up
|
||
|
||
# SLCAN (candlelite2) — 由应用层直接打开串口，无需 ip link
|
||
# 但需要设置串口权限
|
||
chmod 666 /dev/ttyACM0
|
||
|
||
echo "CAN interfaces ready"
|
||
ip -details link show can0
|
||
ip -details link show can1
|
||
```
|
||
|
||
### 10.2 交叉编译
|
||
|
||
```toml
|
||
# .cargo/config.toml
|
||
[target.armv7-unknown-linux-gnueabihf]
|
||
linker = "arm-linux-gnueabihf-gcc"
|
||
rustflags = [
|
||
"-C", "target-cpu=cortex-a7",
|
||
"-C", "target-feature=+neon",
|
||
]
|
||
```
|
||
|
||
```bash
|
||
# 构建
|
||
cargo build --release --target armv7-unknown-linux-gnueabihf
|
||
|
||
# 部署
|
||
scp target/armv7-unknown-linux-gnueabihf/release/rm-app root@t153:/opt/rm/
|
||
scp -r config/ root@t153:/opt/rm/
|
||
```
|
||
|
||
### 10.3 运行时配置
|
||
|
||
```toml
|
||
# config/robot.toml
|
||
[realtime]
|
||
gimbal_priority = 80
|
||
shooter_priority = 75
|
||
chassis_priority = 70
|
||
mlock = true
|
||
|
||
[can]
|
||
can0_bitrate = 1000000
|
||
can1_bitrate = 1000000
|
||
slcan_device = "/dev/ttyACM0"
|
||
slcan_bitrate = 1000000
|
||
|
||
[gimbal]
|
||
yaw_pid = { kp = 10.0, ki = 0.5, kd = 0.1 }
|
||
pitch_pid = { kp = 12.0, ki = 0.6, kd = 0.15 }
|
||
imu_can_id = 0x100
|
||
|
||
[shooter]
|
||
friction_pid = { kp = 5.0, ki = 0.2, kd = 0.05 }
|
||
trigger_pid = { kp = 3.0, ki = 0.1, kd = 0.02 }
|
||
|
||
[chassis]
|
||
speed_pid = { kp = 8.0, ki = 0.3, kd = 0.05 }
|
||
```
|
||
|
||
---
|
||
|
||
## 11. 执行 Agent 工作流
|
||
|
||
### 11.1 阶段划分
|
||
|
||
| 阶段 | 任务 | 验收 |
|
||
|------|------|------|
|
||
| **P0: 骨架** | 创建 workspace + 所有 crate 的空骨架 | `cargo build` 通过 |
|
||
| **P1: 协议层** | 实现 `rm-hal` + `rm-protocol` | 单元测试 + 模糊测试通过 |
|
||
| **P2: 驱动层** | 实现 `rm-drivers`（SocketCAN + SLCAN） | vcan 回环测试通过 |
|
||
| **P3: 设备层** | 实现 `rm-devices` | mock 测试通过 |
|
||
| **P4: 控制层** | 实现 `rm-control` | PID 单元测试通过 |
|
||
| **P5: 仿真层** | 实现 `rm-sim` | 整定测试通过 |
|
||
| **P6: 应用层** | 实现 `rm-app` | 在 T153 上运行 |
|
||
| **P7: 实机联调** | 在真实机器人上验证 | 所有电机可控 |
|
||
|
||
### 11.2 每个阶段的交付物
|
||
|
||
- **代码**：所有实现文件
|
||
- **测试**：单元测试 + 集成测试
|
||
- **文档**：模块级文档注释 + 必要的 README
|
||
- **配置**：对应的 TOML 配置文件
|
||
|
||
### 11.3 质量门禁
|
||
|
||
每个阶段完成后必须通过：
|
||
|
||
1. `cargo build --workspace` 无警告
|
||
2. `cargo test --workspace` 全绿
|
||
3. `cargo clippy --workspace -- -D warnings` 无警告
|
||
4. `cargo fmt --check` 通过
|
||
5. 新增代码有文档注释
|
||
6. 关键路径有测试覆盖
|
||
|
||
### 11.4 决策点
|
||
|
||
以下决策需要人工确认后才能继续：
|
||
|
||
- **D1**: delta10 的 CAN ID 和协议模式（需查阅数据手册）
|
||
- **D2**: GM6020 和 M3508 的具体 ID 分配（需确认硬件接线）
|
||
- **D3**: candlelite2 的串口设备名（需在 T153 上 `ls /dev/ttyACM*`）
|
||
- **D4**: 是否启用 E907 异构核（基于 P7 的抖动测试结果）
|
||
|
||
---
|
||
|
||
## 12. 附录
|
||
|
||
### 12.1 术语表
|
||
|
||
| 术语 | 含义 |
|
||
|------|------|
|
||
| SLCAN | Serial CAN，一种通过串口传输 CAN 帧的 ASCII 协议 |
|
||
| SocketCAN | Linux 内核的 CAN 子系统 |
|
||
| SLCAN 固件 | candlelite2 上运行的非 SocketCAN 固件 |
|
||
| vcan | Linux 虚拟 CAN 接口，用于测试 |
|
||
| FDILINK | delta10 的一种 CAN 协议模式 |
|
||
| ZN | Ziegler-Nichols，一种 PID 整定方法 |
|
||
|
||
### 12.2 参考链接
|
||
|
||
- [socketcan-rs](https://github.com/socketcan-rs/socketcan-rs)
|
||
- [slcan_fd](https://crates.io/crates/slcan_fd)
|
||
- [discrete_pid](https://crates.io/crates/discrete_pid)
|
||
- [candleLight](https://github.com/candle-usb/candleLight_fw)
|
||
- [DETA10 手册](https://fdisystems.github.io/DETA10_Docs/)
|
||
|
||
### 12.3 变更日志
|
||
|
||
| 版本 | 日期 | 变更 |
|
||
|------|------|------|
|
||
| 1.0 | 2026-09-23 | 初始版本，基于 T153 + SLCAN + delta10 确认 |
|
||
|
||
---
|
||
|
||
**文档结束**
|
||
|
||
执行 Agent 应按照第 11 节的阶段划分逐步推进，每个阶段完成后提交验收报告，遇到决策点（D1-D4）时暂停并请求人工确认。
|