FitCircle - Points array from RobTargets/JointTargets?

RAPID Programming
1

I’m wanting to utilize Fitcircle in determining the center of a circle feature, using the X,Y,Z point data extracted from a series of robtargets, or joint targets if that’s easier.

The idea being the targets would be produced using Search1D, X,Y,Z data extracted and then used to find the center of the circle feature searched.

Robot searches the part, pints recorded, center of circle calculated and other routines run from there.

My programming skills are very, very, basic so you might have to dumb things down for me. Let me know if you need any other details, robot details etc. I’ll do my best to fill in the gaps.

2

Check out this thread

Keke seems to have written a function called Circle3P that can find the center of a circle from three points, but the forum seems to have formatted it in a funky way. I’ve tried to clean it up but there may be some small syntactical mistakes, check it out:

MODULE MainModule
CONST num nEPSILON:=1e-6;
PROC main()
VAR pos p1;
VAR pos p2;
VAR pos p3;
VAR pos c;
VAR pos a;
VAR num r;
VAR bool b;
p1:=[200,0,100];
p2:=[0,0,100];
p3:=[0,200,100];
b:=Circle3P(p1,p2,p3,c,a,r);
Stop;
ENDPROC
FUNC num Max(num n1, num n2)
!--------------------------------!Return maximum of two numbers !--------------------------------!
IF n1 > n2 THEN
RETURN n1;
ELSE
RETURN n2;
ENDIF
ENDFUNC
FUNC num PosLength(pos p)
!-------------------------------- !Length of a vector (pos) !--------------------------------!
VAR num nL;
nL:=sqrt(Max(0,p.xp.x+p.yp.y+p.zp.z));
RETURN nL;
ENDFUNC
FUNC pos CrossProd(pos p1,pos p2)
!--------------------------------!Calculate cross product for vectors p1 and p2.!--------------------------------!
VAR pos px;
px:=[0,0,0];
px.x:=p1.yp2.z-p2.yp1.z;
px.y:=p2.xp1.z-p1.xp2.z;
px.z:=p1.xp2.y-p2.xp1.y;
RETURN px;
ENDFUNC
FUNC bool Circle3P(pos p1,pos p2,pos p3,INOUT pos center,INOUT pos axis,INOUT num radius)
!--------------------------------!Calculate center point, axis and radius for a circle formed by three point p1, p2 and p3. Axis is unit vector.!--------------------------------!
!Returns True if found, False if points are on a on a single line (infinite radius).!--------------------------------!
VAR pos v12;
VAR pos v23;
VAR pos px;
VAR pos p12mid;
VAR pos p23mid;
VAR pos n12;
VAR pos n23;
VAR pos diff;
VAR num l;
VAR dnum A{3,3};
VAR dnum b{3};
VAR dnum x{3};
center:=[0,0,0];
axis:=[0,0,0];
radius:=0;
v12:=p2-p1;
v23:=p3-p2;
px:=CrossProd(v12,v23);
l:=PosLength(px);
IF l<nEPSILON THEN
RETURN FALSE;
ENDIF
px:=(1.0/l)px;
axis:=px;
p12mid:=p1+0.5v12;
p23mid:=p2+0.5v23;
n12:=CrossProd(v12,axis);
n23:=CrossProd(v23,axis);
A:=[[NumToDnum(v12.x),NumToDnum(v12.y),NumToDnum(v12.z)],
[NumToDnum(v23.x),NumToDnum(v23.y),NumToDnum(v23.z)],[NumToDnum(axis.x),NumToDnum(axis.y),NumToDnum(axis.z)]];
b:=[NumToDnum(DotProd(v12,p12mid)),NumToDnum(DotProd(v23,p23mid)),NumToDnum(DotProd(axis,p2))];
center:=[DnumToNum(x{1}),DnumToNum(x{2}),DnumToNum(x{3})];
diff:=center-p1;
radius:=PosLength(diff);
RETURN TRUE;
ENDFUNC
ENDMODULE