BEGIN PGM 403 MM
FN 18: SYSREAD Q0 = ID2000 NR10 IDX4158
FN 9: IF +Q0 EQU +1 GOTO LBL 254
FN 18: SYSREAD Q0 = ID990 NR2 IDX16
FN 9: IF +Q0 EQU +0 GOTO LBL 254
Q198 = 403
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
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
; A-Achse
FN 10: IF +Q312 NE +4 GOTO LBL 30
L IA+Q50 R0 FQ59
FN 20: WAIT FOR SYNC
FN 9: IF +Q337 EQU +0 GOTO LBL 199
FN 18: SYSREAD Q6 = ID270 NR1 IDX4
FN 11: IF +Q305 GT +0 GOTO LBL 40
CALL LBL 60
FN 17: SYSWRITE ID 502 NRQ305 IDX4 = +Q6
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 40
FN 9: IF +Q303 EQU +0 GOTO LBL 50
FN 17: SYSWRITE ID 502 NRQ305 IDX4 = +Q6
FN 18: SYSREAD Q0 = ID530 NR1
FN 10: IF +Q305 NE +Q0 GOTO LBL 199
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 50
FN 17: SYSWRITE ID 500 NRQ305 IDX4 = +Q6
FN 9: IF +0 EQU +0 GOTO LBL 199
; B-Achse
LBL 30
FN 10: IF +Q312 NE +5 GOTO LBL 31
L IB+Q50 R0 FQ59
FN 20: WAIT FOR SYNC
FN 9: IF +Q337 EQU +0 GOTO LBL 199
FN 18: SYSREAD Q6 = ID270 NR1 IDX5
FN 11: IF +Q305 GT +0 GOTO LBL 41
CALL LBL 60
FN 17: SYSWRITE ID 502 NRQ305 IDX5 = +Q6
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 41
FN 9: IF +Q303 EQU +0 GOTO LBL 51
FN 17: SYSWRITE ID 502 NRQ305 IDX5 = +Q6
FN 18: SYSREAD Q0 = ID530 NR1
FN 10: IF +Q305 NE +Q0 GOTO LBL 199
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 51
FN 17: SYSWRITE ID 500 NRQ305 IDX5 = +Q6
FN 9: IF +0 EQU +0 GOTO LBL 199
; C-Achse
LBL 31
FN 10: IF +Q312 NE +6 GOTO LBL 32
L IC+Q50 R0 FQ59
FN 20: WAIT FOR SYNC
FN 9: IF +Q337 EQU +0 GOTO LBL 199
FN 18: SYSREAD Q6 = ID270 NR1 IDX6
FN 11: IF +Q305 GT +0 GOTO LBL 42
CALL LBL 60
FN 17: SYSWRITE ID 502 NRQ305 IDX6 = +Q6
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 42
FN 9: IF +Q303 EQU +0 GOTO LBL 52
FN 17: SYSWRITE ID 502 NRQ305 IDX6 = +Q6
FN 18: SYSREAD Q0 = ID530 NR1
FN 10: IF +Q305 NE +Q0 GOTO LBL 199
FN 17: SYSWRITE ID 530 NR1 =+Q305
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 52
FN 17: SYSWRITE ID 500 NRQ305 IDX6 = +Q6
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 32
FN 14: ERROR= 1019
;** COPY ACTIVE LINE INTO LINE 0
LBL 60
;ALLE PRESETWERTE LESEN
FN 18: SYSREAD Q0 = ID530 NR1
FN 10: IF +Q0 NE -1 GOTO LBL 61
Q0 = 0
LBL 61
FN 18: SYSREAD QL1 = ID503 NRQ0 IDX1
FN 18: SYSREAD QL2 = ID503 NRQ0 IDX2
FN 18: SYSREAD QL3 = ID503 NRQ0 IDX3
FN 18: SYSREAD QL4 = ID503 NRQ0 IDX4
FN 18: SYSREAD QL5 = ID503 NRQ0 IDX5
FN 18: SYSREAD QL6 = ID503 NRQ0 IDX6
FN 18: SYSREAD QL7 = ID503 NRQ0 IDX7
FN 18: SYSREAD QL8 = ID503 NRQ0 IDX8
FN 18: SYSREAD QL9 = ID503 NRQ0 IDX9
FN 18: SYSREAD QL10 = ID504 NRQ0
FN 17: SYSWRITE ID 503 NRQ305 IDX1 =+QL1
FN 17: SYSWRITE ID 503 NRQ305 IDX2 =+QL2
FN 17: SYSWRITE ID 503 NRQ305 IDX3 =+QL3
FN 17: SYSWRITE ID 503 NRQ305 IDX4 =+QL4
FN 17: SYSWRITE ID 503 NRQ305 IDX5 =+QL5
FN 17: SYSWRITE ID 503 NRQ305 IDX6 =+QL6
FN 17: SYSWRITE ID 503 NRQ305 IDX7 =+QL7
FN 17: SYSWRITE ID 503 NRQ305 IDX8 =+QL8
FN 17: SYSWRITE ID 503 NRQ305 IDX9 =+QL9
FN 17: SYSWRITE ID 504 NRQ305 =+QL10
LBL 0
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 199
Q150 = Q50
FN 17: SYSWRITE ID 990 NR1 =+0
FN 17: SYSWRITE ID 990 NR2 =+0
LBL 254
END PGM 403 MM
