107 lines
2.8 KiB
Markdown
107 lines
2.8 KiB
Markdown
# 螺杆/工具盘定位 ModbusTCP 协议文档
|
||
|
||
## 1. 概述
|
||
|
||
- 协议: Modbus TCP
|
||
- 服务端默认端口: `5020`
|
||
- 当前实现使用保持寄存器
|
||
- 地址说明均为保持寄存器地址
|
||
|
||
## 2. 寄存器定义
|
||
|
||
| 地址 | 长度 | 访问 | 说明 |
|
||
|------|------|------|------|
|
||
| `0` | 1 | 写 | 触发检测，`1=螺杆检测`，`2=工具盘检测`，写入后服务端会自动清零 |
|
||
| `1` | 1 | 读 | 工作状态，`0=空闲`，`1=检测中`，`2=完成`，`3=异常` |
|
||
| `2` | 2 | 写 | 机械臂 `X`，`float32` |
|
||
| `4` | 2 | 写 | 机械臂 `Y`，`float32` |
|
||
| `6` | 2 | 写 | 机械臂 `Z`，`float32` |
|
||
| `8` | 2 | 写 | 机械臂 `RX`，`float32` |
|
||
| `10` | 2 | 写 | 机械臂 `RY`，`float32` |
|
||
| `12` | 2 | 写 | 机械臂 `RZ`，`float32` |
|
||
| `14` | 2 | 读 | 结果 `X`，`float32` |
|
||
| `16` | 2 | 读 | 结果 `Y`，`float32` |
|
||
| `18` | 2 | 读 | 结果 `Z`，`float32` |
|
||
| `20` | 2 | 读 | 结果 `RX`，`float32` |
|
||
| `22` | 2 | 读 | 结果 `RY`，`float32` |
|
||
| `24` | 2 | 读 | 结果 `RZ`，`float32` |
|
||
|
||
说明:
|
||
|
||
- 地址 `2~13` 为机械臂当前位姿（眼在手上模式），触发前写入
|
||
- 地址 `14~25` 为检测结果输出 `XYZ RX RY RZ`
|
||
- 每个 `float32` 占用 2 个保持寄存器，字序为高字在前、低字在后
|
||
- 当前版本只输出首个目标位姿
|
||
- 当没有结果或检测失败时，`14~25` 地址返回全 0
|
||
|
||
## 3. 触发方式
|
||
|
||
### 3.1 写入机械臂位姿
|
||
|
||
触发前，先将机械臂当前位姿写入地址 `2~13`。
|
||
|
||
### 3.2 触发螺杆检测
|
||
|
||
向地址 `0` 写入:
|
||
|
||
```text
|
||
1
|
||
```
|
||
|
||
### 3.3 触发工具盘检测
|
||
|
||
向地址 `0` 写入:
|
||
|
||
```text
|
||
2
|
||
```
|
||
|
||
工具盘检测会调用 `sx_getLocationPlatePose` 算法，输出定位盘中心位姿。
|
||
|
||
## 4. 工作状态说明
|
||
|
||
| 状态值 | 含义 |
|
||
|--------|------|
|
||
| `0` | 空闲，可接收下一次触发 |
|
||
| `1` | 检测中 |
|
||
| `2` | 检测完成，结果已写入地址 `14~25` |
|
||
| `3` | 检测异常或当前请求不被支持 |
|
||
|
||
## 5. 位姿数据格式
|
||
|
||
### 机械臂位姿（输入，地址 2~13）
|
||
|
||
| 字段 | 地址范围 |
|
||
|------|----------|
|
||
| `X` | `2~3` |
|
||
| `Y` | `4~5` |
|
||
| `Z` | `6~7` |
|
||
| `RX` | `8~9` |
|
||
| `RY` | `10~11` |
|
||
| `RZ` | `12~13` |
|
||
|
||
### 检测结果（输出，地址 14~25）
|
||
|
||
| 字段 | 地址范围 |
|
||
|------|----------|
|
||
| `X` | `14~15` |
|
||
| `Y` | `16~17` |
|
||
| `Z` | `18~19` |
|
||
| `RX` | `20~21` |
|
||
| `RY` | `22~23` |
|
||
| `RZ` | `24~25` |
|
||
|
||
其中:
|
||
|
||
- 螺杆检测输出为机械臂坐标系下的 `XYZ + RX/RY/RZ`
|
||
- 工具盘检测输出为机械臂坐标系下的定位盘中心 `XYZ + RX/RY/RZ`（法向量转欧拉角）
|
||
|
||
## 6. 典型流程
|
||
|
||
1. 读取地址 `1`，确认当前为 `0`
|
||
2. 将机械臂当前位姿写入地址 `2~13`
|
||
3. 向地址 `0` 写入 `1` 或 `2`
|
||
4. 轮询地址 `1`
|
||
5. 当地址 `1=2` 时，读取地址 `14~25`
|
||
6. 当地址 `1=3` 时，本次检测失败
|