;-> AUTOWRITE IN G54:A,B IF NOT ZERO
;>>>>> G54:A = R18 <<<<<
;>>>>> G54:B = R19 <<<<<

EXTERN SETKOS(INT)

DEFINE TOL_ANGLE AS 2

DEFINE XSTART AS 25
DEFINE YSTART AS 25
DEFINE ZSTART AS 250
DEFINE XDIFF AS 600
DEFINE YDIFF AS 1000
DEFINE FEED_FAST AS 13000.0

R18=0
R19=0
;$P_UIFR[1,X,RT] = 0
;$P_UIFR[1,Y,RT] = 0
;MSG(<<($P_UIFR[1,Y,RT]==0))
STOPRE
;M0

SETKOS(0)
SOFT
TRAORI
G54


;P1
G90 G1 A0 B0 C10 F500
G90 G1 X=XSTART Y=YSTART Z=ZSTART F=FEED_FAST
G4 F2
CYC_MESSZ1
R10=-R9
STOPRE
M01

;P2
G90 G1 A0 B0 C10 F500
G90 G1 X=XSTART Y=YSTART+YDIFF Z=ZSTART F=FEED_FAST
G4 F2
CYC_MESSZ1
R11=-R9
STOPRE
M01

;P3
G90 G1 A0 B0 C10 F500
G90 G1 X=XSTART+XDIFF Y=YSTART+YDIFF Z=ZSTART F=FEED_FAST
G4 F2
CYC_MESSZ1
R12=-R9
STOPRE
M01


; BERECHNE
R18=ATAN((R11-R10)/XDIFF)
R19= - ATAN((R12-R11)/YDIFF)
STOPRE


; SETZE
IF ($P_UIFR[1,X,RT] == 0) AND ($P_UIFR[1,Y,RT] == 0)
    IF (ABS(R18) < TOL_ANGLE) AND (ABS(R19) < TOL_ANGLE) 
        $P_UIFR[1,X,RT] = R18; + $P_UIFR[1,X,RT]
        $P_UIFR[1,Y,RT] = R19; + $P_UIFR[1,Y,RT]
        STOPRE
    ELSE
        MSG("AUSSERHALB TOLERANZ, WERTE A, B: "<<R18 <<" " <<R19)
        M0
    ENDIF
ELSE
    MSG("G54:A,B NOT WRITTEN, RESET IT MANUALLY")
    M0
ENDIF

SETKOS(1)

M17