;>>>>> IN SETKOS.MPF:LASER        <<<<<
;>>>>> $MC_ROBX_TTCFL_POS[0] = -X_L = -R8 <<<<<
;>>>>> $MC_ROBX_TTCFL_POS[2] = -Y_L = -R9 <<<<<

EXTERN SETKOS(INT)

DEFINE XSTART AS -70
DEFINE YSTART AS 630
DEFINE ZSTART AS 200
DEFINE XPROBE AS 250.0
DEFINE FEED_FAST AS 11000.0
DEFINE FEED_MESS AS 200.0

DEFINE THETA1 AS 10
DEFINE THETA2 AS 90
DEFINE THETA3 AS 130

H1=370.0
R1=0
R2=0
R3=0
R4=0

SETKOS(0)
TRAORI
G54

G90 G1 A0 B0 C=THETA1 F1700
G90 G1 X=XSTART Y=YSTART Z=ZSTART F=FEED_FAST
G91 G1 X=XPROBE F=FEED_MESS MEAS=1
STOPRE
R1=$AA_MW[X]
STOPRE
M01

G90 G1 A0 B0 C=THETA2 F1700
G90 G1 X=XSTART Y=YSTART Z=ZSTART F=FEED_FAST
G91 G1 X=XPROBE F=FEED_MESS MEAS=1
STOPRE
R2=$AA_MW[X]
STOPRE
M01

G90 G1 A0 B0 C=THETA3 F1700
G90 G1 X=XSTART Y=YSTART Z=ZSTART F=FEED_FAST
G91 G1 X=XPROBE F=FEED_MESS MEAS=1
STOPRE
R3=$AA_MW[X]
STOPRE
M01



; BERECHNE
;(* Compute X_L *)
R8 = ((R2 - R1) * (-SIN(THETA3) + SIN(THETA1)) - (R3 - R1) * (-SIN(THETA2) + SIN(THETA1))) / ((COS(THETA2) - COS(THETA1)) * (-SIN(THETA3) + SIN(THETA1)) - (COS(THETA3) - COS(THETA1)) * (-SIN(THETA2) + SIN(THETA1)))

;(* Compute Y_L *)
R9 = ((R2 - R1) - (COS(THETA2) - COS(THETA1)) * R8) / (-SIN(THETA2) + SIN(THETA1))



G90 G1 A0 B0 C0 F1700
HOME
SETKOS(1)


M17