107 lines
3.1 KiB
Markdown
107 lines
3.1 KiB
Markdown
|
# 轮胎孔定位 ModbusTCP 协议
|
|||
|
|
|
|||
|
|
## 1. 概述
|
|||
|
|
|
|||
|
|
- 协议：Modbus TCP
|
|||
|
|
- 默认端口：`502`
|
|||
|
|
- 当前实现使用保持寄存器
|
|||
|
|
- 内部实现端口：`5020`
|
|||
|
|
|
|||
|
|
## 2. 寄存器定义
|
|||
|
|
|
|||
|
|
| 地址 | 长度 | 访问 | 说明 |
|
|||
|
|
|---:|---|---|---|
|
|||
|
|
| `0` | 1 | 写 | 触发检测，`1=轮胎孔检测` |
|
|||
|
|
| `1` | 1 | 读 | 工作状态，`0=空闲`，`1=检测中`，`2=完成`，`3=异常` |
|
|||
|
|
| `2` | 2 | 写 | 机械臂当前 `X`，`float32` |
|
|||
|
|
| `4` | 2 | 写 | 机械臂当前 `Y`，`float32` |
|
|||
|
|
| `6` | 2 | 写 | 机械臂当前 `Z`，`float32` |
|
|||
|
|
| `8` | 2 | 写 | 机械臂当前 `A`，`float32` |
|
|||
|
|
| `10` | 2 | 写 | 机械臂当前 `B`，`float32` |
|
|||
|
|
| `12` | 2 | 写 | 机械臂当前 `C`，`float32` |
|
|||
|
|
| `14` | 2 | 读 | 目标点 `X`，`float32` |
|
|||
|
|
| `16` | 2 | 读 | 目标点 `Y`，`float32` |
|
|||
|
|
| `18` | 2 | 读 | 目标点 `Z`，`float32` |
|
|||
|
|
| `20` | 2 | 读 | 目标点 `A`，`float32` |
|
|||
|
|
| `22` | 2 | 读 | 目标点 `B`，`float32` |
|
|||
|
|
| `24` | 2 | 读 | 目标点 `C`，`float32` |
|
|||
|
|
|
|||
|
|
说明：
|
|||
|
|
|
|||
|
|
- `2~13` 为机械臂当前位姿输入
|
|||
|
|
- `14~25` 为目标点输出（6 个 float32）
|
|||
|
|
- 每个 `float32` 占 2 个保持寄存器
|
|||
|
|
- 当前仅输出第一个目标结果
|
|||
|
|
|
|||
|
|
## 3. 姿态输入/输出顺序
|
|||
|
|
|
|||
|
|
`A/B/C` 的实际含义由网络配置中的 `poseOutputOrder` 统一控制。
|
|||
|
|
|
|||
|
|
支持顺序：
|
|||
|
|
|
|||
|
|
- `RX-RY-RZ`
|
|||
|
|
- `RX-RZ-RY`
|
|||
|
|
- `RY-RX-RZ`
|
|||
|
|
- `RY-RZ-RX`
|
|||
|
|
- `RZ-RX-RY`
|
|||
|
|
- `RZ-RY-RX`
|
|||
|
|
|
|||
|
|
同一个配置同时作用于：
|
|||
|
|
|
|||
|
|
- Modbus 输入 `A/B/C`
|
|||
|
|
- Modbus 输出 `A/B/C`
|
|||
|
|
|
|||
|
|
例如，当 `poseOutputOrder = RZ-RY-RX` 时：
|
|||
|
|
|
|||
|
|
- 地址 `8~13` 表示输入 `RZ/RY/RX`
|
|||
|
|
- 地址 `20~25` 表示目标点输出 `RZ/RY/RX`
|
|||
|
|
|
|||
|
|
## 4. 工作状态说明
|
|||
|
|
|
|||
|
|
| 状态值 | 含义 |
|
|||
|
|
|---:|---|
|
|||
|
|
| `0` | 空闲 |
|
|||
|
|
| `1` | 检测中 |
|
|||
|
|
| `2` | 检测完成 |
|
|||
|
|
| `3` | 异常或请求不支持 |
|
|||
|
|
|
|||
|
|
## 5. 触发流程
|
|||
|
|
|
|||
|
|
1. 确认地址 `1` 当前为 `0`（空闲）
|
|||
|
|
2. 将机械臂当前位姿写入 `2~13`
|
|||
|
|
3. 向地址 `0` 写入 `1`（轮胎孔检测）
|
|||
|
|
4. 轮询地址 `1`
|
|||
|
|
5. 当地址 `1=2` 时，读取 `14~25`（目标点）
|
|||
|
|
6. 机械臂运动到目标点 `14~25`
|
|||
|
|
7. 当地址 `1=3` 时，本次检测失败
|
|||
|
|
|
|||
|
|
## 6. 结果语义
|
|||
|
|
|
|||
|
|
- **轮胎孔检测**输出：机械臂坐标系下的孔中心 `目标点(X/Y/Z) + A/B/C`
|
|||
|
|
- 孔姿态由法向量（normDir）经手眼变换后提取欧拉角
|
|||
|
|
- 检测结果可能包含多个孔，Modbus 仅输出第一个
|
|||
|
|
|
|||
|
|
说明：
|
|||
|
|
|
|||
|
|
- 内部姿态求解顺序由 `eulerOrder` 控制
|
|||
|
|
- `poseOutputOrder` 只控制寄存器中姿态字段的排列顺序
|
|||
|
|
- 位置字段（`X/Y/Z`）保留 3 位小数，单位 mm
|
|||
|
|
- 姿态字段 `A/B/C` 保留 3 位小数，单位 deg
|
|||
|
|
|
|||
|
|
## 7. 算法说明
|
|||
|
|
|
|||
|
|
使用 `workpieceHolePositioning` 算法库：
|
|||
|
|
|
|||
|
|
- **轮胎孔定位**：`sx_getTireHolePose()`
|
|||
|
|
- 输入：扫描线数据 + 角点检测参数（cornerParam）+ 地面标定参数（groundCalibPara）+ 轮胎参数（tireParam: diameter, thickness）
|
|||
|
|
- 输出：孔中心位置列表 + 法向量方向列表（vector\<WD_HolePositionInfo\>）
|
|||
|
|
|
|||
|
|
## 8. TCP 文本协议
|
|||
|
|
|
|||
|
|
除 ModbusTCP 外，还支持 TCP 文本协议（默认端口 `7800`）：
|
|||
|
|
|
|||
|
|
- 请求：`D1_X_Y_Z_A_B_C\r\n`（D=轮胎孔检测）
|
|||
|
|
- 数字表示相机编号（1 或 2）
|
|||
|
|
- 成功响应：`count_X_Y_Z_A_B_C\r\n`（6 个 float：目标点）
|
|||
|
|
- 失败响应：`0\r\n`
|