PERS num cam_x:=0.00189328;
PERS num cam_y:=3E-9;
PERS num count:=1;
PERS point testPoint:=[705.005,-7.10543E-15];
PERS num calib_x:=10; ! 标定移动时的x距离
PERS num calib_y:=10; ! 标定移动时的y距离
PERS num calib_rz:=5; ! 标定移动时的绕tool0旋转的角度
PROC main()
MoveJ p0,v1000,fine,tool0\WObj:=wobj0;
count:=1;
rCalib p0,calib_x,calib_y,calib_rz;
stop;
rCam2;
calNewPoint [cam_x,cam_y],testPoint;
ENDPROC
PROC rCalib(robtarget p,num x,num y,num angle)
MoveL offs(p,-x,-y,0),v100,fine,tool0;
rCam;
MoveL offs(p,0,-y,0),v100,fine,tool0;
rCam;
MoveL offs(p,x,-y,0),v100,fine,tool0;
rCam;
MoveL offs(p,x,0,0),v100,fine,tool0;
rCam;
MoveL offs(p,0,0,0),v100,fine,tool0;
rCam;
MoveL offs(p,-x,0,0),v100,fine,tool0;
rCam;
MoveL offs(p,-x,y,0),v100,fine,tool0;
rCam;
MoveL offs(p,0,y,0),v100,fine,tool0;
rCam;
MoveL offs(p,x,y,0),v100,fine,tool0;
rCam;
MoveL offs(p,0,0,0),v100,fine,tool0;
MoveL rotWobj(p,0,0,-angle),v100,fine,tool0;
rCam;
MoveL rotWobj(p,0,0,0),v100,fine,tool0;
rCam;
MoveL rotWobj(p,0,0,angle),v100,fine,tool0;
rCam;
calibCal;
ENDProc
PROC calibCal()
VAR pos posCir{3};
VAR point realPoint;
VAR pos posCenter;
VAR num r;
VAR pos n;
calculateAffineMatrix\PointNum:=9,img_points,real_points,Affine;
calNewPoint img_points{10},realPoint;
posCir{1}.x:=realPoint.x;
posCir{1}.y:=realPoint.y;
calNewPoint img_points{11},realPoint;
posCir{2}.x:=realPoint.x;
posCir{2}.y:=realPoint.y;
calNewPoint img_points{12},realPoint;
posCir{3}.x:=realPoint.x;
posCir{3}.y:=realPoint.y;
fitcircle posCir,posCenter,r,n;
Affine{3}:=Affine{3}+(posCir{2}.x-posCenter.x);
Affine{6}:=Affine{6}+(posCir{2}.y-posCenter.y);
ENDPROC
FUNCrobtarget rotWobj(robtarget p,num rx,num ry,num rz)
VAR orient o;
o:=p.rot;
o:=orientzyx(rz,ry,rx)*o;
p.rot:=o;
RETURN p;
ENDFUNC
PROC rCam2()
waittime 0.5;
PulseDO do_cam;
waittime 0.5;
ENDPROC
PROC rCam()
VAR robtarget p;
rCam2;
p:=CRobT();
img_points{count}.x:=cam_x;
img_points{count}.y:=cam_y;
real_points{count}.x:=p.trans.x;
real_points{count}.y:=p.trans.y;
incr count;
ENDPROC