EPSON 利用CCD图像处理包标定工具坐标系

Function CalcTool
Boolean found
Real x, y, u
Real x0, y0, u0
Real xTool, yTool, rTool, theta
Tool 0

 VRun findTip
VGet findTip.tip.RobotXYU, found, x, y, u
If found Then
' Get the TOOL 0 position
x0 = CX(P*)
y0 = CY(P*)
u0 = CU(P*)
' Calculate initial tool offsets
' X and Y distance from tip found with vision
' to center of U axis
xTool = x - x0
yTool = y - y0
' Calculate angle at initial offsets
theta = Atan2(xTool, yTool)
' Calculate angle of tool when U is at zero degrees
theta = theta - DegToRad(u0)
' Calculate tool radius
rTool = Sqr(xTool * xTool + yTool * yTool)
' Calculate final tool offsets
xTool = Cos(theta) * rTool
yTool = Sin(theta) * rTool
' Set the tool
TLSet 1, XY(xTool, yTool, 0, 0)
EndIf
Fend