69 lines
1.2 KiB
Plaintext
69 lines
1.2 KiB
Plaintext
;>>>>> 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
|