feat(hardware): 接入 SocketCAN 与智身模组协议，补温度诊断和指令看门狗
- hardware: 新增 can_bus.h / joint_module_protocol.h，实现使能/失能/运行帧与 12 字节反馈帧编解码；ReadHardware/WriteHardware 落地，控制字按协议语义映射 （kErrorReset 按重新使能处理，kGetStatus 无需请求） - hardware: 温度/驱动状态/电压经 /joint_diagnostics 发布（默认 10 Hz，可配置） - hardware: 指令看门狗，100 ms 无指令转阻尼，避免上游挂掉后重发最后一帧 - hardware: 限幅与归一化边界分离——[joint_limits] 是唯一实际限幅，加载期校验 其不超过 [motor.*] 的归一化边界；力矩按峰值扭矩夹紧，kp/kd 越界夹紧并告警 - hardware: require_can 开关，无 CAN 时可只发状态（冒烟测试用） - remote: 1002/4 关节按 M20 的 16 槽上报（swing→HipY、wheel→Wheel，其余占位 0）， 1002/5 温度改由 /joint_diagnostics 提供 - deploy: 启动时序等订阅者出现再发（ROS1 无 latch，否则整段丢失） - msgs: 新增 JointDiagnostics - docs: 智身模组 CANFD 协议整理、待办清单
This commit is contained in:
@@ -0,0 +1,8 @@
|
||||
# 文档索引
|
||||
|
||||
| 文档 | 内容 |
|
||||
| --- | --- |
|
||||
| [zhishen_joint_module_canfd.md](zhishen_joint_module_canfd.md) | 智身关节模组 CANFD 通信协议（整理自厂家手册 V1.2）：通信参数、帧格式、使能/失能/运行/置零指令、状态与故障位、数值编码、各型号阈值 |
|
||||
| [TODO.md](TODO.md) | 待办清单：每项写清原因与验收方式，已完成项在文末 |
|
||||
|
||||
厂家原始资料：《智身关节模组使用手册 V1.2（CANFD 版）》，2026-04-22。
|
||||
@@ -0,0 +1,88 @@
|
||||
# TODO
|
||||
|
||||
本文件记录**当前会话中确认但本期不做**的事项。按优先级分组，每项尽量写清「为什么」和「验收方式」，
|
||||
避免下次重新推导。已完成的部分见各包 README。
|
||||
|
||||
## P0：安全链路（真机跑 RL 前必须处理）
|
||||
|
||||
- [ ] **异常数据接入 deploy 的保护链路**
|
||||
硬件已在 `/joint_diagnostics` 提供温度/状态字/电压（10 Hz），但 deploy 侧仍是常量：
|
||||
`robot_interface.h` 的 `GetMotorTemperture()` 返回 30 ℃、`GetDriverStatusWord()` 返回全 1，
|
||||
所以 `SafeController` 的过温检查和驱动报错检查恒不触发。
|
||||
做法：`InvictaInterface` 订阅 `/joint_diagnostics` 并覆盖这两个虚函数。
|
||||
注意：协议 Byte9 的 bit0 是「使能中」，而 `IsDriverStatusNormal()` 判断 `status_word != 1` 报错，
|
||||
失能但健康的电机回 `0x00` 会误判；建议在接口层归一化成「无故障位就填 1」。
|
||||
验收：人为把某个关节温度改成 101 ℃，deploy 进入 `safe_control_mode=2` 并切 `kJointDamping`。
|
||||
|
||||
- [ ] **诊断超时判断**
|
||||
诊断话题 10 Hz，`SafeController` 读缓存值。若 hardware 进程死掉，温度会停在最后一帧的「正常」。
|
||||
需要在接口层记录诊断时间戳，超时（如 1 s）按异常处理。
|
||||
验收：杀掉 hardware 节点后 deploy 进入安全状态。
|
||||
|
||||
- [ ] **异常上报 1002/3 的数据源**
|
||||
remote 的 `ErrorDoc` 恒为空列表，因为 `MotionState.msg` 没有错误字段。
|
||||
建议给 `MotionState` 加 `uint32 error_code` + `uint16 error_component`（位域与
|
||||
`SafeController::robot_error_state_` 对齐，component 用 M20 槽号），remote 再映射成
|
||||
`0x8001/0x8002/0x8020/0x8012` 等协议错误码。
|
||||
验收：模拟驱动器故障，App 收到非空 `ErrorList` 且 component 指向正确关节。
|
||||
|
||||
- [ ] **IMU 数据源缺失**
|
||||
`/imu/data` 目前发的是单位四元数、`orientation_covariance[0] = 0`（声称有效），
|
||||
但协议里没有任何 IMU 字段，硬件桥也没有读 IMU。
|
||||
按本项目自己的约定，拿不到姿态时应把 `orientation_covariance[0]` 置 `-1`；
|
||||
改之前要确认 deploy 的 `[rl].require_imu` 会不会因此拒绝进 RL。
|
||||
|
||||
## P1：标定与配置一致性（上真机前）
|
||||
|
||||
- [ ] **`dir`/`offset` 的施加方未定**
|
||||
hardware 现在收发的都是**原始电机量**，`dir`/`offset_deg` 由 deploy 的 `InvictaInterface` 施加；
|
||||
但 hardware 的 README 原设计与 `hardware_bridge.h` 注释写的是「由本节点施加」。
|
||||
两边都施加时 `dir=±1` 会二次相乘抵消、`offset` 会翻倍（当前 offset 全 0，尚未暴露）。
|
||||
二选一并同步修改另一侧配置与文档；同时注意 `[joint_limits]` 在两份 TOML 里重复维护。
|
||||
|
||||
- [ ] **反馈帧仲裁 ID 需真机确认**
|
||||
`ReadHardware` 按「反馈使用关节自身 CANID」实现（手册只写了从站 CANID 1~6）。
|
||||
上电后 `candump can0` 核对；若不是 1~4，需要改匹配逻辑。
|
||||
|
||||
- [ ] **`lie_height = 0` 与位置限位矛盾**
|
||||
`invicta_deploy.toml` 里 `lie_height = 0.00` 反解 `θ = acos(0) = 90° = 1.571 rad`，
|
||||
超过同文件 `[joint_limits].upper = 1.40`。确认是笔误还是限位需要放宽。
|
||||
|
||||
- [ ] **轮的位置限位占位值**
|
||||
`[joint_limits]` 中轮的 `lower/upper = ±100` 既不在归一化边界内，也不表示限幅
|
||||
（轮不支持位置控制，手册 §4.4）。加载校验时已跳过，但语义上应改成 ±12.5 或显式标注「仅占位」。
|
||||
|
||||
- [ ] **摆臂速度限幅复核**
|
||||
`vel_limit = 22.40 rad/s` 是额定 15.5 的 1.45 倍，比轮的 100/81.2（1.23 倍）更激进。
|
||||
如果当初是估的，建议按实测重新定。
|
||||
|
||||
- [ ] **位置指令越界目前静默夹紧**
|
||||
力矩和 kp/kd 越界会告警，位置不会。策略动作顶到限位时可能值得提示一次（注意别刷屏）。
|
||||
|
||||
## P2：协议状态上报的剩余占位
|
||||
|
||||
- [ ] `MotionStatus` 的 `LinearX / LinearY / Height / RemainMile`：需要里程计、运动学估计与电池数据。
|
||||
- [ ] `1002/5` 的电池（电压/电量/温度/serial）、CPU、GPS：无数据源。
|
||||
- [ ] `Charge / HES / Sleep`：没有充电状态、硬急停信号与休眠管理。
|
||||
- [ ] `HipY` 的正负号未与真实 App/M20 对照：如果 App 只是显示数值没问题，若渲染姿态需要核对。
|
||||
- [ ] 未实现指令：照明灯、休眠设置/查询、自主充电、速度指令（导航模式）、导航任务（Pro）。
|
||||
|
||||
## P3：工程与清理
|
||||
|
||||
- [ ] **启动时序的 `kEnable` 可能被采样合并**
|
||||
deploy 已改为「等订阅者再发」且 hardware 在 `kMotorControl` 未使能时会补发使能，功能上已安全；
|
||||
但 hardware 循环线程处理 `kErrorReset` 的使能连发会阻塞约 80 ms，期间可能观察不到 `kEnable`。
|
||||
若要严格逐帧落地，可把连发改成分段非阻塞发送。
|
||||
- [ ] `[motor.gains].bench_*` 已被指令里的 kp/kd 取代，确认是否删除。
|
||||
- [ ] 构建镜像 `localhost/custom-ros-noetic` 缺 toml++，目前靠挂载外部安装树 + `CMAKE_PREFIX_PATH`。
|
||||
可考虑在镜像里装 toml++，或在仓库内 vendored。
|
||||
- [ ] 真机联调清单：`ip link set canX ... fd on` 权限、`roslaunch` 启动顺序、双路差分驱动方向。
|
||||
|
||||
## 已完成（本会话）
|
||||
|
||||
- [x] SocketCAN FD 收发 + 智身模组协议编解码（使能/失能/运行、12 字节反馈解码）
|
||||
- [x] 温度读回 → `/joint_diagnostics`（默认 10 Hz，可配置）→ remote 填 1002/5
|
||||
- [x] 1002/4 的 8 → 16 槽映射（swing→HipY、wheel→Wheel，HipX/Knee 占位 0）
|
||||
- [x] 力矩/速度/位置限幅，kp/kd 越界夹紧并告警；限幅与归一化边界分离 + 加载期校验
|
||||
- [x] 指令看门狗（100 ms 无指令转阻尼）
|
||||
- [x] 容器内构建 + 冒烟测试；修复启动时序丢失与订阅回调阻塞
|
||||
@@ -0,0 +1,282 @@
|
||||
# 智身关节模组 CANFD 通信协议
|
||||
|
||||
> 整理自《智身关节模组使用手册 V1.2（CANFD 版）》，智身新创（苏州）智能科技有限公司，2026-04-22。
|
||||
> 本文覆盖手册第 3 章协议主体，并补充第 1 章阻抗控制模型与第 2 章中影响协议使用的约束。
|
||||
> 手册 V1.2 的变更：修改 P65、P85MAX-S 转矩阈值；新增 W110-S、P100MAX-S 关节通信阈值。
|
||||
|
||||
## 1. 适用范围
|
||||
|
||||
- 硬件：支持 CAN FD 的关节模组控制器与主控设备。
|
||||
- 场景：电机控制、状态监测、参数配置。
|
||||
- 控制模式：**阻抗控制模式**。电机使能后默认进入该模式，内部最终执行力矩闭环：
|
||||
|
||||
```
|
||||
力矩指令 = Kp × (位置指令 − 反馈位置)
|
||||
+ Kd × (速度指令 − 反馈速度)
|
||||
+ 前馈力矩指令
|
||||
```
|
||||
|
||||
该模型无积分项。Kp 越大位置刚度越高；Kd 越大阻尼越强、动态响应越慢。位置、速度、转矩、阻尼四种“模式”只是同一模型在不同参数配置下的表现：
|
||||
|
||||
| 控制方式 | 参数配置 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| 位置控制 | 给定 `P_cmd`，`Kp > 0`，建议 `Kd > 0` | 围绕目标位置的弹性约束，允许位置偏差；轮毂电机不支持 |
|
||||
| 速度控制 | 给定 `V_cmd`，`Kd > 0`，`Kp = 0` | Kd 相当于比例系数，存在稳态速度误差 |
|
||||
| 转矩控制 | 给定 `T_ff`，其余为 0 | 直接输出恒定转矩；负载小于给定转矩时会持续加速 |
|
||||
| 阻尼控制 | 仅 `Kd > 0`，其余为 0 | 输出与速度反向、大小成比例的阻力，不约束位置 |
|
||||
|
||||
## 2. CANFD 通信参数
|
||||
|
||||
| 类别 | 参数项 | 参数说明 |
|
||||
| --- | --- | --- |
|
||||
| 参数配置 | 波特率（仲裁段） | 1 Mbps |
|
||||
| 参数配置 | 波特率（数据段） | 5 Mbps |
|
||||
| 参数配置 | 帧格式 | 标准帧 |
|
||||
| 参数配置 | 数据长度（主站发送） | 64 字节（可配置） |
|
||||
| 参数配置 | 数据长度（关节返回） | 12 字节 |
|
||||
| CANID | 主站 CANID | `0x00` |
|
||||
| CANID | 从站关节 CANID | `0x0X`（X = 1~6） |
|
||||
| CANID | 关节出厂默认 CANID | `0x03` |
|
||||
| 通信速率 | 主机与关节通信速率 | 最大支持 1 kHz |
|
||||
|
||||
多字节字段全部为 **小端（低字节在前）**。下文示例中的 `00 80` 即 uint16 的 `0x8000`。
|
||||
|
||||
## 3. 帧格式
|
||||
|
||||
### 3.1 主机下发帧
|
||||
|
||||
一帧最多控制 6 个关节，每个关节占 10 字节，按 CANID 顺序排列：
|
||||
|
||||
| Byte | 功能 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| Byte0–Byte1 | MSGID | 主机发送，从机原样返回；用于匹配请求帧，不用可填 0 |
|
||||
| Byte2–Byte11 | CANID=1 指令数据 | 见第 4 节 |
|
||||
| Byte12–Byte21 | CANID=2 指令数据 | 同上 |
|
||||
| Byte22–Byte31 | CANID=3 指令数据 | 同上 |
|
||||
| Byte32–Byte41 | CANID=4 指令数据 | 同上 |
|
||||
| Byte42–Byte51 | CANID=5 指令数据 | 同上 |
|
||||
| Byte52–Byte61 | CANID=6 指令数据 | 同上 |
|
||||
| Byte62–Byte63 | 保留 | 手册未定义 |
|
||||
|
||||
每个关节块的起始字节为 `2 + 10 × (CANID − 1)`。
|
||||
|
||||
**下发长度**可按最大 CANID 裁剪：
|
||||
|
||||
| 最大关节 CANID | 下发最小字节数 |
|
||||
| ---: | ---: |
|
||||
| 1 | 12 |
|
||||
| 2 | 24 |
|
||||
| 3 | 32 |
|
||||
| 4 | 48 |
|
||||
| 5 / 6 | 64 |
|
||||
|
||||
- 最大 CANID=3 时下发 Byte0–Byte31（32 字节）；最大 CANID=1 时下发 Byte0–Byte11（12 字节）。
|
||||
- 只控制不连续关节时，仍需按最大 CANID 的长度下发：例如只控 CANID=3，则发 64 字节，仅给 Byte22–Byte31 赋值，其余填 0 或保持上次值；控 CANID=1 和 4 时，给 Byte2–Byte11 与 Byte32–Byte41 赋值。
|
||||
- CANID=4 时手册要求 48 字节，而实际数据只用到 Byte41（42 字节），以手册要求为准。
|
||||
|
||||
### 3.2 关节反馈帧
|
||||
|
||||
对所有指令，关节均返回固定的 12 字节状态帧：
|
||||
|
||||
| Byte | 功能 | 说明 |
|
||||
| --- | --- | --- |
|
||||
| Byte0–Byte1 | MSGID | 等于主机发送值 |
|
||||
| Byte2–Byte3 | 关节输出端当前位置 | uint16，见第 5 节映射 |
|
||||
| Byte4–Byte5 | 关节输出端当前速度 | uint16，rad/s |
|
||||
| Byte6–Byte7 | 关节输出端当前扭矩 | uint16，N·m |
|
||||
| Byte8 | 电机当前温度 | uint8，`0~255` 对应 `−40~215 °C`（即 `T = raw − 40`） |
|
||||
| Byte9 | 电机当前状态码 | 位定义见第 6 节 |
|
||||
| Byte10 | 电机当前电压 | uint8，`0~255` 直接对应 `0~255 V` |
|
||||
| Byte11 | 预留 | — |
|
||||
|
||||
## 4. 指令数据（每关节 10 字节）
|
||||
|
||||
### 4.1 电机使能
|
||||
|
||||
| Byte | 说明 | 取值 |
|
||||
| --- | --- | --- |
|
||||
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
|
||||
| Byte11 | 使能指令码 | `0xFC` |
|
||||
|
||||
示例（使能所有关节，MSGID = `0x0100`）：
|
||||
|
||||
```
|
||||
Byte0-1 : 00 01
|
||||
Byte2-11 : FF FF FF FF FF FF FF FF FF FC
|
||||
Byte12-21 : FF FF FF FF FF FF FF FF FF FC
|
||||
...
|
||||
Byte52-61 : FF FF FF FF FF FF FF FF FF FC
|
||||
Byte62-63 : xx xx
|
||||
```
|
||||
|
||||
### 4.2 电机失能
|
||||
|
||||
| Byte | 说明 | 取值 |
|
||||
| --- | --- | --- |
|
||||
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
|
||||
| Byte11 | 使能指令码 | `0xFD` |
|
||||
|
||||
失能后所有 MOS 管关闭，电机线圈处于断开状态。
|
||||
|
||||
### 4.3 电机运行
|
||||
|
||||
必须在使能之后发送。运行指令块内容：
|
||||
|
||||
| Byte | 说明 | 取值 |
|
||||
| --- | --- | --- |
|
||||
| Byte2–Byte3 | 位置指令 | uint16，`0~65535` 映射到 `P_MIN~P_MAX`（rad），`0x8000` = 0 rad |
|
||||
| Byte4–Byte5 | 速度指令 | uint16，`0~65535` 映射到 `V_MIN~V_MAX`（rad/s），`0x8000` = 0 rad/s |
|
||||
| Byte6–Byte7 | 刚度 Kp 指令 | uint16，`0~65535` 映射到 `0~KP_MAX`，`0x0000` = 0 |
|
||||
| Byte8–Byte9 | 阻尼 Kd 指令 | uint16，`0~65535` 映射到 `0~KD_MAX`，`0x0000` = 0 |
|
||||
| Byte10–Byte11 | 前馈力矩指令 | uint16，`0~65535` 映射到 `T_MIN~T_MAX`（N·m），`0x8000` = 0 N·m |
|
||||
|
||||
示例（所有关节 0 位置、0 速度、0 Kp、0 Kd、0 力矩）：
|
||||
|
||||
```
|
||||
Byte0-1 : 00 01
|
||||
Byte2-11 : 00 80 00 80 00 00 00 00 00 80
|
||||
Byte12-21 : 00 80 00 80 00 00 00 00 00 80
|
||||
...
|
||||
Byte52-61 : 00 80 00 80 00 00 00 00 00 80
|
||||
```
|
||||
|
||||
### 4.4 设置当前位置为机械零位
|
||||
|
||||
| Byte | 说明 | 取值 |
|
||||
| --- | --- | --- |
|
||||
| Byte2–Byte10 | 固定指令码（9 字节） | `0xFF` × 9 |
|
||||
| Byte11 | 零位指令码 | `0xEF` |
|
||||
|
||||
设置成功且关节角度未变化时，当前反馈位置为 0。电机在任意位置的上电角度上报范围为 `(−π, π)` rad。
|
||||
|
||||
> 完成机械零点标定后，关节具备单圈绝对位置记忆：断电及人为转动输出端后重新上电，仍可恢复当前单圈绝对位置。**轮毂电机不支持该功能，也不支持位置控制。**
|
||||
|
||||
## 5. 数值编码
|
||||
|
||||
所有物理量以 uint16 线性映射。手册写作 `0~65535 → MIN~MAX`。
|
||||
|
||||
编码（物理量 → 原始值）：
|
||||
|
||||
```
|
||||
raw = round((x - MIN) / (MAX - MIN) × 65535)
|
||||
```
|
||||
|
||||
解码（原始值 → 物理量）：
|
||||
|
||||
```
|
||||
x = MIN + raw / 65535 × (MAX - MIN)
|
||||
```
|
||||
|
||||
说明：
|
||||
|
||||
- 位置、速度、前馈力矩为对称范围，因此 `0x8000`（32768）对应 0。按上式，`raw = 32768` 时 `x ≈ MIN + 0.5000076 × (MAX − MIN)`，与 0 的偏差来自 65536 个码值无法整除；工程上通常直接按 `32768` 作为零点，或用 `(raw − 32768) / 32768 × MAX` 计算对称量。
|
||||
- Kp、Kd 为非负范围，`0x0000` 对应 0。
|
||||
- 下发前必须按关节型号的 `P/V/T/KP/KD` 阈值限幅，见第 8 节。
|
||||
|
||||
Python 参考实现：
|
||||
|
||||
```python
|
||||
def encode(x, lo, hi):
|
||||
r = round((x - lo) / (hi - lo) * 65535)
|
||||
return max(0, min(65535, r))
|
||||
|
||||
def decode(raw, lo, hi):
|
||||
return lo + raw / 65535 * (hi - lo)
|
||||
|
||||
def pack_u16(v):
|
||||
return bytes((v & 0xFF, (v >> 8) & 0xFF)) # 小端
|
||||
|
||||
def run_block(p, v, kp, kd, t, lim):
|
||||
return b"".join((
|
||||
pack_u16(encode(p, lim["p_min"], lim["p_max"])),
|
||||
pack_u16(encode(v, lim["v_min"], lim["v_max"])),
|
||||
pack_u16(encode(kp, 0.0, lim["kp_max"])),
|
||||
pack_u16(encode(kd, 0.0, lim["kd_max"])),
|
||||
pack_u16(encode(t, lim["t_min"], lim["t_max"])),
|
||||
))
|
||||
```
|
||||
|
||||
## 6. 状态码与故障保护
|
||||
|
||||
Byte9 状态码按位定义（1 有效）：
|
||||
|
||||
| bit | 功能 | 触发条件 | 恢复措施 |
|
||||
| ---: | --- | --- | --- |
|
||||
| 0 | 电机使能 | 使能为 1，失能为 0 | — |
|
||||
| 1 | 过压故障 | 母线电压超过阈值 | 重启或重新使能 |
|
||||
| 2 | 过流故障 | 电流超过阈值 | 重启或重新使能 |
|
||||
| 3 | 过温故障 | 电机温度超过阈值 | 重启或重新使能；状态位低于 85 °C 自动恢复 |
|
||||
| 4 | 过速故障 | 输出端速度超过阈值 | 重启或重新使能 |
|
||||
| 5 | 编码器故障 | 编码器测量失效 | 重启或重新使能 |
|
||||
| 6 | 欠压故障 / 预驱故障 | 母线电压低于阈值，或检测到预驱故障 | 重启或重新使能 |
|
||||
| 7 | 预留 | — | — |
|
||||
|
||||
发生故障时电机会自动失能，同时对应状态位置 1。主站应持续检查 Byte9 的 bit1–bit6，非零即进入故障处理。
|
||||
|
||||
## 7. 关节型号与阈值
|
||||
|
||||
| 关节型号 | P_MAX | P_MIN | V_MAX | V_MIN | T_MAX | T_MIN | KP_MAX | KD_MAX |
|
||||
| --- | ---: | ---: | ---: | ---: | ---: | ---: | ---: | ---: |
|
||||
| P55 | 12.5 | −12.5 | 65 | −65 | 30 | −30 | 500 | 5 |
|
||||
| P65 | 12.5 | −12.5 | 65 | −65 | 50 | −50 | 500 | 5 |
|
||||
| P65MAX-S | 12.5 | −12.5 | 65 | −65 | 100 | −100 | 500 | 5 |
|
||||
| P85MAX-S | 12.5 | −12.5 | 65 | −65 | 300 | −300 | 500 | 50 |
|
||||
| W150-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
|
||||
| W190-S | 12.5 | −12.5 | 200 | −200 | 50 | −50 | 500 | 5 |
|
||||
| W110-S | 12.5 | −12.5 | 300 | −300 | 30 | −30 | 500 | 5 |
|
||||
| P100MAX-S | 12.5 | −12.5 | 65 | −65 | 650 | −650 | 500 | 50 |
|
||||
|
||||
单位：位置 rad，速度 rad/s，转矩 N·m；Kp、Kd 无量纲。轮毂电机（W 系列）速度阈值明显高于摆臂电机。
|
||||
|
||||
### 7.1 额定参数与额定速度（厂家参数页补充）
|
||||
|
||||
| 型号 | 额定功率 (W) | 额定扭矩 (N·m) | 峰值扭矩 (N·m) | 推算额定速度 (rad/s) |
|
||||
| --- | ---: | ---: | ---: | ---: |
|
||||
| P55 | 78.5 | 6.5 | 21 | 12.1 |
|
||||
| P65 | 330 | 11.5 | 48 | 28.7 |
|
||||
| P65MAX-S | 330 | 21.3 | 75 | 15.5 |
|
||||
| P85MAX-S | 706 | 43 | 180 | 16.4 |
|
||||
| W150-S | 495 | 3.5 | 11 | 141.4 |
|
||||
| W190-S | 706 | 8.7 | 28.68 | 81.2 |
|
||||
|
||||
额定速度 = 额定功率 / 额定扭矩。它和上表的 `V_MAX` 不是一回事：`V_MAX` 是**通信编码阈值**（决定 uint16 的满量程），额定速度是持续工作能力。本机取的三层关系：
|
||||
|
||||
| 关节 | 额定速度 | 人为限幅（`[joint_limits].vel_limit`） | 通信阈值（`[motor.*].v_max`） |
|
||||
| --- | ---: | ---: | ---: |
|
||||
| 摆臂 P65MAX-S | 15.5 | 22.4 | 65 |
|
||||
| 轮 W190-S | 81.2 | 100 | 200 |
|
||||
|
||||
人为限幅略高于额定、远低于阈值；实际下发按 `min(阈值, 限幅)` 饱和。注意阈值/额定的比值并不统一（P65MAX-S ≈ 4.2×，W190-S ≈ 2.5×），不能用其中一个推另一个。
|
||||
|
||||
## 8. 调试给定值参考
|
||||
|
||||
手册给出的典型增益（`P_cmd`, `V_cmd`, `T_ff`, `Kp`, `Kd`）：
|
||||
|
||||
| 关节型号 | 位置控制 | 速度控制 | 转矩控制 |
|
||||
| --- | --- | --- | --- |
|
||||
| P55 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
|
||||
| P65 | `3.14, 0, 0, 2, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
|
||||
| P65MAX-S | `3.14, 0, 0, 3, 1` | `0, 5, 0, 0, 2` | `0, 0, 1, 0, 0` |
|
||||
| P85MAX-S | `3.14, 0, 0, 5, 1` | `0, 5, 0, 0, 2` | `0, 0, 3, 0, 0` |
|
||||
| W150-S | — | `0, 5, 0, 0, 0.1` | `0, 0, 0.3, 0, 0` |
|
||||
| W190-S | — | `0, 5, 0, 0, 0.5` | `0, 0, 0.2, 0, 0` |
|
||||
|
||||
阻尼控制只需 `Kd > 0`，其余参数置 0。
|
||||
|
||||
使用注意：
|
||||
|
||||
1. 目标位置与当前位置差值不宜过大，否则会产生瞬时大转矩和大电流；建议给定值缓慢增加。
|
||||
2. `Kp`、`Kd` 的比值不宜过大；速度控制时 `Kd` 不宜过大。
|
||||
3. 速度项启动转矩由速度给定与 `Kd` 决定，突变会产生电流峰值或振荡。
|
||||
4. 转矩控制下若负载小于给定转矩，转速会一直上升到最大转速甚至失控。
|
||||
5. 写入参数后需断电重启才能生效；上位机装入参数后需再次“写入参数”。
|
||||
|
||||
## 9. 通信时序要点
|
||||
|
||||
1. 上电后先发使能（`0xFC`），再发运行指令；失能（`0xFD`）后线圈断开。
|
||||
2. 需要绝对零位时，在装配完成后发一次置零（`0xEF`），之后反馈位置以该点为 0。
|
||||
3. 主机发送周期最大支持 1 kHz；实际控制周期按上层需求设定，并保证每帧携带同一 MSGID 以匹配反馈。
|
||||
4. 一帧可同时控制最多 6 个关节；不同 CANID 的关节共用一个 64 字节帧，按 `2 + 10 × (CANID − 1)` 定位各自数据块。
|
||||
5. 每个关节独立返回 12 字节状态帧，主站需按 MSGID 关联请求与反馈。
|
||||
6. 反馈中的位置、速度、转矩均使用与下发相同的 `MIN~MAX` 映射，解码时必须使用对应关节型号的阈值。
|
||||
7. ⚠️ 编码满量程只能取第 7 节的型号阈值，不能取摇杆/工作范围。台架脚本曾用 `float_to_uint(v, V_MAX=10, ...)` 下发轮速，而 W190-S 的阈值是 ±200：同一 raw 值固件按 ±200 解码，实际轮速被放大 20 倍。工作上限（如 100）应作为独立限幅先夹紧，再按阈值编码。
|
||||
Reference in New Issue
Block a user