GrabBag/App/ScrewPosition/Doc/螺杆定位ModbusTCP协议文档.md

螺杆定位 / 工具盘定位 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	写	机械臂当前 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
• 工具盘检测输出：机械臂坐标系下的定位盘中心 接近点(AX/AY/AZ) + 目标点(X/Y/Z) + A/B/C

说明：

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