# 螺杆/工具盘定位 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` 时，本次检测失败