1 TOOL P_NTOOL ‘该语句写成 TOOL （0,0,0,0,0,0） 也得分
2 ‘直交坐标系下， 移动机器人， 将工具顶尖对准固定顶尖
3 P0=P_FBC
4 P91=P0*(0,0,0,0,0,90)
5 MVS P91
6 PTL = P_Zero
7 ‘直交坐标系下， 移动机器人的 XY 坐标， 使得工具顶尖再次对准固定顶尖
8 P90=P_Fbc
9 PT = INV(P90)*P0
10 PTL.x=(PT.y+PT.x)/2
11 PTL.x=(PT.y-PT.x)/2
12 TOOL PTL
13 ‘直交坐标系下， 移动机器人的 C 轴， 确认工具顶尖是否与固定顶尖重合，若重合， 则工具坐标测算成果， 否则重头开始。