GrabBag/App/DiscHolePose/Doc/砂轮盘孔定位ModbusTCP协议文档.md

砂轮盘孔定位 / 砂轮盘架子定位 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