Ye, i was talking about the radius of the arc ( in millimeters).
Anyway this is the code i wrote to calculate the radius of my arc.
The first part is just the resolution of the system to find out a,b,c. after that i calculate the arc.
i tryed with the traslation of the origin to the first point of the arc to "fix" the approssimation of the robot and now it is working good.
GLOBAL DEFFCT REAL RArc(X1:IN,Y1:IN,X2:IN,Y2:IN,X3:IN,Y3:IN)
;Questa funzione serve a trovare il raggio di una circonferenza passante per 3 punti.
;EQUAZIONE CIRCONFERENZA PASSANTE PER 3 PUNTI FORMA ESPLICITA
;X^2+Y^2+ax+by+c=0
;--
;EQUAZIONE CIRCONFERENZA PASSANTE PER 3 PUNTI FORMA IMPLICITA
;(X-Xo)^2+(Y-Yo)^2=r^2
;--
;RELAZIONE MATEMATICA TRA FORMA IMPLICITA ED ESPLICITA
;X^2+Y^2-2*Xo*X-2*Yo*Y+Xo^2+Y0^2-r
;-2Xo=a
;-2Yo=b
;X^2+Y^2-r^2=c
;--
;FORMULA ANGOLO CONOSCENDO CATETO ADIACENTE E IPOTENUSA
;ACOS(CATETO/IPOTENUSA)
DECL REAL X1,Y1,X2,Y2,X3,Y3
DECL REAL X1_EL2,Y1_EL2,X2_EL2,Y2_EL2,X3_EL2,Y3_EL2,Xo_EL2,Yo_EL2
DECL REAL a,b,c,r
DECL REAL Xo,Yo
;--
;FORMULA DELL'EQUAZIONE DI UNA CIRCONFERENZA
;{X1^2+Y1^2+aX1+Y1+c=0
;--
;METTO I TERMINI NEL SISTEMA
;{X1^2+Y1^2+aX1+bY1+c=0
;{X2^2+Y2^2+aX2+bY2+c=0
;{X3^2+Y3^2+aX3+bY3+c=0
;--
;TRASLO LO ZERO DELLA BASE AL PRIMO PUNTO DEL ARCO ED ESTRAPOLO LE COORDINATE DEGLI ALTRI DUE PUNTI
X2=X2-X1
Y2=Y2-Y1
X3=X3-X1
Y3=Y3-Y1
X1=0
Y1=0
;ELEVO I TERMINI DA INSERIRE NEL SISTEMA ( POW_NUM() --> FUNZIONE PER ELEVARE A POTENZA)
X1_EL2=POW_NUM(X1,2)
Y1_EL2=POW_NUM(Y1,2)
X2_EL2=POW_NUM(X2,2)
Y2_EL2=POW_NUM(Y2,2)
X3_EL2=POW_NUM(X3,2)
Y3_EL2=POW_NUM(Y3,2)
;--
;ISOLO LE INCOGNITE (ULTIMO PASSAGGIO DEL SISTEMA)
;c=-X1^2-Y1^2-X1*a-Y1*b
;b=(-X2^2-Y2^2-X2a+X1^2+Y1^2+X1a))/(Y2-Y1)
;a=(X2^2*Y3+Y2^2*Y3-X1^2*Y3-Y1^2*Y3+X1^2*Y2-X1^2*Y1+Y1^2*Y2-X2^2*Y1-Y2^2*Y1+X1^2*Y1-X3^2*Y2+X3^2*Y1-Y3^2*Y2+Y3^2*Y1)/(X3*Y2-X3*Y1-X2*Y3+X1*Y3-X1*Y2+X2*Y1)
;--
;CALCOLO a,b,c partendo da "a" con i calcoli (avendo già b e c isolate),poi sostituisco in "b" e successivamente in "c" per trovare tutte le incognite.
;Scrivo direttamente la parte risolutiva del sistema
a=(X2_EL2*Y3+Y2_EL2*Y3-X1_EL2*Y3-Y1_EL2*Y3+X1_EL2*Y2-X1_EL2*Y1+Y1_EL2*Y2-X2_EL2*Y1-Y2_EL2*Y1+X1_EL2*Y1-X3_EL2*Y2+X3_EL2*Y1-Y3_EL2*Y2+Y3_EL2*Y1)/(X3*Y2-X3*Y1-X2*Y3+X1*Y3-X1*Y2+X2*Y1)
b=(-X2_EL2-Y2_EL2-a*X2+X1_EL2+Y1_EL2+a*X1)/(Y2-Y1)
c=-X1_EL2-Y1_EL2-a*X1-b*Y1
;CALCOLO X E Y D'ORIGINE
Xo=-a/2
Yo=-b/2
;ELEVO A POTENZA Xo,Yo
Xo_EL2=POW_NUM(Xo,2)
Yo_EL2=POW_NUM(Yo,2)
;CALCOLO IL RAGGIO
r=SQRT(Xo_EL2+Yo_EL2-c)
RETURN r
ENDFCT
Display More