# 砂轮盘孔定位 / 砂轮盘架子定位 ModbusTCP 协议

## 1. 概述

- 协议：Modbus TCP
- 默认端口：`502`
- 当前实现使用保持寄存器

## 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 | 写 | 机械臂当前 `A`，`float32` |
| `10` | 2 | 写 | 机械臂当前 `B`，`float32` |
| `12` | 2 | 写 | 机械臂当前 `C`，`float32` |
| `14` | 2 | 读 | 接近点 `AX`，`float32` |
| `16` | 2 | 读 | 接近点 `AY`，`float32` |
| `18` | 2 | 读 | 接近点 `AZ`，`float32` |
| `20` | 2 | 读 | 接近点 `A`，`float32`（与目标姿态相同） |
| `22` | 2 | 读 | 接近点 `B`，`float32`（与目标姿态相同） |
| `24` | 2 | 读 | 接近点 `C`，`float32`（与目标姿态相同） |
| `26` | 2 | 读 | 目标点 `X`，`float32` |
| `28` | 2 | 读 | 目标点 `Y`，`float32` |
| `30` | 2 | 读 | 目标点 `Z`，`float32` |
| `32` | 2 | 读 | 目标点 `A`，`float32` |
| `34` | 2 | 读 | 目标点 `B`，`float32` |
| `36` | 2 | 读 | 目标点 `C`，`float32` |

说明：

- `2~13` 为机械臂当前位姿输入
- `14~25` 为 **接近点** 输出（机械臂先运动到这里）
- `26~37` 为 **目标点** 输出（从接近点沿工具 Z 轴直线下压到此）
- 接近点与目标点的 `A/B/C` 姿态字段值相同
- 每个 `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`
- 地址 `32~37` 表示目标点输出 `RZ/RY/RX`

## 4. 工作状态说明

| 状态值 | 含义 |
|---:|---|
| `0` | 空闲 |
| `1` | 检测中 |
| `2` | 检测完成 |
| `3` | 异常或请求不支持 |

## 5. 触发流程

1. 确认地址 `1` 当前为 `0`
2. 将机械臂当前位姿写入 `2~13`
3. 向地址 `0` 写入 `1`（砂轮盘孔检测）或 `2`（砂轮盘架子检测）
4. 轮询地址 `1`
5. 当地址 `1=2` 时，读取 `14~37`（接近点 + 目标点）
6. 机械臂先运动到接近点 `14~25`，再保持姿态沿工具 Z 轴直线下压到目标点 `26~37`
7. 当地址 `1=3` 时，本次检测失败

## 6. 结果语义

- **砂轮盘孔检测**输出：机械臂坐标系下的孔中心 `接近点(AX/AY/AZ) + 目标点(X/Y/Z) + A/B/C`
  - 孔姿态由法向量（normDir）计算：Pitch 由 Z 分量与水平面夹角确定，Yaw 由 X/Y 分量确定，Roll 固定为 0
- **砂轮盘架子检测**输出：机械臂坐标系下的架子中心 `接近点(AX/AY/AZ) + 目标点(X/Y/Z) + A/B/C`
  - 架子姿态由三轴向量（pose_x/pose_y/pose_z）构建旋转矩阵后提取欧拉角

说明：

- 内部姿态求解顺序由 `eulerOrder` 控制
- `poseOutputOrder` 只控制寄存器中姿态字段的排列顺序
- 接近点与目标点姿态一致；机械臂到达接近点后沿工具 Z 轴直线下压到目标点
- 接近点 = 目标点在 Eye 坐标系下、沿"Z 轴方向"偏移 `approachOffset`，再经手眼矩阵变换到机器人坐标系得到
- `approachOffset` 每台相机独立配置（手眼标定页的"接近点偏移(mm)"）；为 0 时接近点与目标点重合

## 7. 算法说明

使用 `workpieceHolePositioning` 算法库：

- **砂轮盘孔定位**：`sx_getDiscHolePose()`
  - 输入：扫描线数据 + 角点检测参数（cornerParam）
  - 输出：孔中心位置 + 法向量方向

- **砂轮盘架子定位**：`sx_getDiscRackCenterPosition()`
  - 输入：扫描线数据 + 角点检测参数（cornerParam）
  - 输出：架子中心位置 + 三轴方向向量（pose_x/pose_y/pose_z）

## 8. TCP 文本协议

除 ModbusTCP 外，还支持 TCP 文本协议（默认端口 `7800`）：

- 请求：`D1_X_Y_Z_A_B_C\r\n`（D=砂轮盘孔检测）或 `R1_X_Y_Z_A_B_C\r\n`（R=砂轮盘架子检测）
  - 数字表示相机编号（1 或 2）
- 成功响应：`1_AX_AY_AZ_A_B_C_X_Y_Z_A_B_C\r\n`（12 个 float：接近点 6 个 + 目标点 6 个）
- 失败响应：`0_0_0_0_0_0_0_0_0_0_0_0\r\n`