BEGIN PGM 403 MM
CALL PGM CHECK_BAMODE.CYC
FN 9: IF +Q169 EQU +1.1 GOTO LBL 254
Q198 = 403
CALL PGM ORIENT.CYC
CALL PGM INIT41X.CYC
CALL PGM INIT.CYC
Q320 = Q320 + Q2 + Q3
FN 10: IF +Q267 NE +0 GOTO LBL "Check_ProbeDir"
FN 14: ERROR= 1042
LBL "Check_ProbeDir"
; *** Y-RICHTUNG **************
FN 12: IF +Q261 LT +Q260 GOTO LBL 130
FN 0: Q260 =+Q261
LBL 130
FN 1: Q1 =+Q109 + +1
FN 18: SYSREAD Q0 = ID270 NR1 IDXQ1
FN 11: IF +Q0 GT +Q260 GOTO LBL 120
L  Z+Q260 R0 FQ59
LBL 120
FN 9: IF +Q272 EQU +1 GOTO LBL 21
FN 9: IF +Q272 EQU +3 GOTO LBL 23
FN 9: IF +Q267 EQU -1 GOTO LBL 20
FN 2: Q22 =+Q264 - +Q320
FN 2: Q32 =+Q266 - +Q320
L  X+Q263  Y+Q22 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 Y+
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 1
L  Z+Q260 FQ59
LBL 1
L  X+Q265  Y+Q32 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 Y+
TCH PROBE 0.1  Z+Q261
FN 9: IF +0 EQU +0 GOTO LBL 92
LBL 20
FN 1: Q22 =+Q264 + +Q320
FN 1: Q32 =+Q266 + +Q320
L  X+Q263  Y+Q22 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 Y-
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 2
L  Z+Q260 FQ59
LBL 2
L  X+Q265  Y+Q32 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 Y-
TCH PROBE 0.1  Z+Q261
FN 9: IF +0 EQU +0 GOTO LBL 92
LBL 21
; *** X-RICHTUNG **************
FN 9: IF +Q267 EQU -1 GOTO LBL 22
FN 2: Q21 =+Q263 - +Q320
FN 2: Q31 =+Q265 - +Q320
L  X+Q21  Y+Q264 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 X+
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 3
L  Z+Q260 FQ59
LBL 3
L  X+Q31  Y+Q266 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 X+
TCH PROBE 0.1  Z+Q261
FN 9: IF +0 EQU +0 GOTO LBL 91
LBL 22
FN 1: Q21 =+Q263 + +Q320
FN 1: Q31 =+Q265 + +Q320
L  X+Q21  Y+Q264 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 X-
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 4
L  Z+Q260 FQ59
LBL 4
L  X+Q31  Y+Q266 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 X-
TCH PROBE 0.1  Z+Q261
FN 9: IF +0 EQU +0 GOTO LBL 91
; *** Z-RICHTUNG *************
LBL 23
FN 9: IF +Q267 EQU -1 GOTO LBL 24
Q261 = Q261 - Q320
L  X+Q263  Y+Q264 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 Z+
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 5
L  Z+Q260 FQ59
LBL 5
L  X+Q265  Y+Q266 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 Z+
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q312 EQU +4 GOTO LBL 91
FN 9: IF +0 EQU +0 GOTO LBL 92
LBL 24
Q261 = Q261 + Q320
L  X+Q263  Y+Q264 R0 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q4 Z-
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q301 EQU +0 GOTO LBL 6
L  Z+Q260 FQ59
LBL 6
L  X+Q265  Y+Q266 FQ59
TCH PROBE 0.0 BEZUGSEBENE Q5 Z-
TCH PROBE 0.1  Z+Q261
FN 9: IF +Q312 EQU +4 GOTO LBL 93
FN 9: IF +Q312 EQU +6 GOTO LBL 95
Q50 = - ATAN ( ( Q5 - Q4 ) / ( Q265 - Q263 ) )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 93
FN 9: IF +Q109 EQU +1 GOTO LBL 190
CALL LBL "DIV_ZERO_AVOID"
Q50 = ATAN ( ( Q264 - Q266 ) / ( Q4 - Q5 ) )
FN 11: IF +Q50 GT +0 GOTO LBL 94
Q50 = - ( Q50 + 90 )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 190
CALL LBL "DIV_ZERO_AVOID"
Q50 = - ATAN ( ( Q263 - Q265 ) / ( Q4 - Q5 ) )
FN 11: IF +Q50 GT +0 GOTO LBL 94
Q50 = - ( Q50 + 90 )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 94
Q50 = ( 90 - Q50 )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 95
FN 9: IF +Q109 EQU +1 GOTO LBL 96
Q50 = - ATAN ( ( Q5 - Q4 ) / ( Q265 - Q263 ) )
FN 9: IF +0 EQU +0 GOTO LBL 99
; FN 14: ERROR= 1016
LBL 96
Q50 = ATAN ( ( Q5 - Q4 ) / ( Q266 - Q264 ) )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 97
Q50 = ATAN ( ( Q5 - Q4 ) / ( Q266 - Q264 ) )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 91
CALL LBL "DIV_ZERO_AVOID"
Q50 = ATAN ( ( Q264 - Q266 ) / ( Q4 - Q5 ) )
FN 9: IF +Q312 EQU +4 GOTO LBL 99
Q50 = Q50 + ( Q380 * - SGN Q50 )
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 92
Q50 = ATAN ( ( Q5 - Q4 ) / ( Q265 - Q263 ) )
FN 9: IF +Q312 EQU +5 GOTO LBL 99
Q50 = Q50 - Q380
LBL 99
L  Z+Q260 R0 FQ59
CALL PGM COMP_BROT.CYC
Q150 = Q50
FN 17: SYSWRITE ID 990 NR1 =+0
FN 17: SYSWRITE ID 990 NR2 =+0
LBL 254
FN 17: SYSWRITE ID 990 NR16 =+0 ;# Spindelkontrolle aus
FN 9: IF +0 EQU +0 GOTO LBL "END"
LBL "DIV_ZERO_AVOID"
FN 10: IF +Q4 NE +Q5 GOTO LBL "DIV_ZERO_AVOID_END"
Q5 = Q5 + 0.0001
LBL "DIV_ZERO_AVOID_END"
LBL 0
LBL "END"
END PGM 403 MM
