CONST robtarget p100:=[[1201.59,1.447676E-05,1075.15],[1.391252E-07,0.1478094,-0.9890159,-2.908804E-08],[-1,0,-1,0],[9E+09,9E+09,9E+09,9E+09,9E+09,9E+09]];VAR robtarget ptmp:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];VAR robtarget ptmp_tool0:=[[0,0,0],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];
! 标准TCPPERS tooldata MyTool:=[TRUE,[[31.792631019,0,229.638935148],[0.945518576,0,0.325568154,0]],[1,[0,0,1],[1,0,0,0],0,0,0]];! 计算出来的新TCPPERS tooldata MyToolNew:=[TRUE,[[21.9529,6.63931,237.327],[0.941921,-0.028375,0.324329,-0.0824075]],[1,[0,0,1],[1,0,0,0],0,0,0]];
PERS num dx:=10;PERS num dy:=10;PERS num drz:=10;    ! 相机发来的偏差值，基于机器人base坐标系
VAR num rx;VAR num ry;VAR num rz;
PROC main()    MyToolNew:=MyTool;    ! 使用标准TCP移动机器人到拍照位置    MoveL p100,v1000,fine,MyTool\WObj:=wobj0;    ! 拍照，并根据相机反馈数据修正TCP    reCalTCP MyTool;    ! 使用新TCP 走到原来位置    MoveL p100,v1000,fine,MyToolNew\WObj:=wobj0;ENDPROC
PROC reCalTCP(inout tooldata tool_std)    waittime 0.5;    ! 拍照并获取相机反馈    ! 相机完成与机器人的手眼标定，相机反馈数据基于机器人base     ! dx, dy, drz are data get from cam
    ptmp:=CRobT(\Tool:=tool_std);    ptmp.trans.x:=ptmp.trans.x+dx;    ptmp.trans.y:=ptmp.trans.y+dy;
    rx:=EulerZYX(\x,ptmp.rot);    ry:=EulerZYX(\y,ptmp.rot);    rz:=EulerZYX(\z,ptmp.rot);    rz:=rz+drz;    ptmp.rot:=OrientZYX(rz,ry,rx);    ! 获取新特征在机器人base下的***位置
    ptmp_tool0:=CRobT(\Tool:=tool0);    !  获取此时法兰盘的位姿    MyToolNew.tframe:=PoseMult(PoseInv([ptmp_tool0.trans,ptmp_tool0.rot]),[ptmp.trans,ptmp.rot]);    ! 计算新TCPENDPROC
PROC rModify()      ! use standard MyTool to teach p100      MoveL p100,v1000,fine,MyTool\WObj:=wobj0;ENDPROC