BEGIN PGM 414 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
CALL PGM CHECK_MP7295.CYC
Q198 = 414
FN 10: IF +Q303 NE +1 GOTO LBL 10
CALL PGM INIT41X.CYC
LBL 10
CALL PGM INIT.CYC
QL0 = Q320
;Q41 = CYC10 + BASROT
Q25 = 0 + Q41 ; Antastrichtung X+
Q26 = 90 + Q41 ; Y+
Q27 = 180 + Q41 ; X-
Q28 = 270 + Q41 ; Y-
Q40 = Q2 ; Wirksamer Radius
Q320 = Q320 + Q40 + Q3 ; Sicherheitsabst.
FN 0: Q267 =+1 ; Antasten Y-Richtung positiv
FN 11: IF +Q297 GT +Q264 GOTO LBL 49
FN 0: Q267 =-1 ; Antasten Y-Richtung negativ
LBL 49
FN 0: Q302 =+1 ; Antasten X-Richtung positiv
FN 11: IF +Q263 GT +Q296 GOTO LBL 51
FN 0: Q302 =-1 ; Antasten X-Richtung negativ
LBL 51
QL11 = Q263     ; Sollposition X1
QL12 = Q264     ; Sollposition Y1
QL14 = Q264     ; Sollposition Y2
QL16 = Q296     ; Sollposition X3
QL17 = Q297     ; Sollposition Y3
QL18 = Q296     ; Sollposition X4
FN 11: IF +Q263 GT +Q296 GOTO LBL "CaseEdge_MajorPos"
Q33 = 2         ; rechts unten
QL13 = Q263 - Q326      ; Sollposition X2
QL19 = Q297 + Q327      ; Sollposition Y4
FN 11: IF +Q297 GT +Q264 GOTO LBL "CaseEdge_End"
Q33 = 3         ; rechts oben
QL19 = Q297 - Q327      ; Sollposition Y4
FN 9: IF +0 EQU +0 GOTO LBL "CaseEdge_End"
LBL "CaseEdge_MajorPos"
Q33 = 1         ; links unten
QL13 = Q263 + Q326      ; Sollposition X2
QL19 = Q297 + Q327      ; Sollposition Y4
FN 11: IF +Q297 GT +Q264 GOTO LBL "CaseEdge_End"
Q33 = 4         ; links oben
QL19 = Q297 - Q327      ; Sollposition Y4
LBL "CaseEdge_End"
; ** Y-RICHTUNG **************
FN 12: IF +Q261 LT +Q260 GOTO LBL 130
FN 14: ERROR= 1016
LBL 130 ; Vorpositionierlogik Z-Achse
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 +Q267 EQU -1 GOTO LBL 20 ; Antasten  Y-?
Q2 = Q264 - Q320 ; Messpunkt 1 Y+
Q31 = Q263 + Q326 ; Messpunkt 2 Y+
FN 9: IF +Q33 EQU +1 GOTO LBL 53 ; Ecke = 1?
Q31 = Q263 - Q326 ; Ecke = 2
LBL 53 ; Antasten Y+
L  X+Q263  Y+Q2 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q26
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q11 =+Q141
FN 0: Q12 =+Q142
FN 9: IF +Q301 EQU +0 GOTO LBL 1
L  Z+Q260 FQ59
LBL 1
L  X+Q31  Y+Q2 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q26
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q13 =+Q141
FN 0: Q14 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 21 ; Fertig mit Y+
LBL 20
Q2 = Q264 + Q320
Q31 = Q263 + Q326
FN 9: IF +Q33 EQU +4 GOTO LBL 54
Q31 = Q263 - Q326
LBL 54 ; Antasten Y-
L  X+Q263  Y+Q2 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q28
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q11 =+Q141
FN 0: Q12 =+Q142
FN 9: IF +Q301 EQU +0 GOTO LBL 2
L  Z+Q260 FQ59
LBL 2
L  X+Q31  Y+Q2 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q28
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q13 =+Q141
FN 0: Q14 =+Q142
LBL 21
L  Z+Q260 FQ59
; ** X-RICHTUNG PLUS **********
FN 9: IF +Q302 EQU -1 GOTO LBL 22
Q1 = Q296 - Q320
Q42 = Q297 + Q327
FN 9: IF +Q33 EQU +1 GOTO LBL 57
Q42 = Q297 - Q327
LBL 57
L  X+Q1  Y+Q297 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q25
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q16 =+Q141
FN 0: Q17 =+Q142
FN 9: IF +Q301 EQU +0 GOTO LBL 3
L  Z+Q260 FQ59
LBL 3
L  X+Q1  Y+Q42 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q25
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q18 =+Q141
FN 0: Q19 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 22
Q1 = Q296 + Q320
Q42 = Q297 + Q327
FN 9: IF +Q33 EQU +2 GOTO LBL 56
Q42 = Q297 - Q327
LBL 56
L  X+Q1  Y+Q297 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q27
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q16 =+Q141
FN 0: Q17 =+Q142
FN 9: IF +Q301 EQU +0 GOTO LBL 4
L  Z+Q260 FQ59
LBL 4
L  X+Q1  Y+Q42 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q27
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q18 =+Q141
FN 0: Q19 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 99
LBL 99
L  Z+Q260 R0 FQ59
Q1 = ABS ( QL13 - QL11 )
Q2 = ABS ( QL14 - QL12 )
FN 11: IF +Q2 GT +Q1 GOTO LBL 45                        ; Nenner groesser Zaehler
Q45 = ATAN ( ( Q14 - Q12 ) / ( Q13 - Q11 ) ) + 90       ; Winkel 1.Kante
FN 9: IF +1 EQU +1 GOTO LBL 46
LBL 45
Q45 = 90 - ATAN ( ( Q13 - Q11 ) / ( Q14 - Q12 ) ) + 90  ; Winkel 1.Kante
LBL 46
Q1 = ABS ( QL19 - QL17 )
Q2 = ABS ( QL16 - QL18 )
FN 11: IF +Q2 GT +Q1 GOTO LBL 47                        ; Nenner groesser Zaehler
Q46 = ATAN ( ( Q16 - Q18 ) / ( Q19 - Q17 ) ) + 180      ; Winkel 2.Kante
FN 9: IF +1 EQU +1 GOTO LBL 48
LBL 47
Q46 = 90 - ATAN ( ( Q19 - Q17 ) / ( Q16 - Q18 ) ) + 180 ; Winkel 2.Kante
LBL 48
Q1 = ( Q11 * COS Q45 ) + ( Q12 * SIN Q45 )
Q2 = ( Q16 * COS Q46 ) + ( Q17 * SIN Q46 )
Q47 = Q45 - 90
; ** Winkelabhngige Fehlerkompensation
FN 11: IF +Q33 GT +1 GOTO LBL 61
; ECKE 1
Q1 = Q1 + Q40
Q2 = Q2 - Q40
FN 9: IF +0 EQU +0 GOTO LBL 60
LBL 61
FN 11: IF +Q33 GT +2 GOTO LBL 62
; ECKE 2
Q1 = Q1 + Q40
Q2 = Q2 + Q40
FN 9: IF +0 EQU +0 GOTO LBL 60
LBL 62
FN 11: IF +Q33 GT +3 GOTO LBL 63
; ECKE 3
Q1 = Q1 - Q40
Q2 = Q2 + Q40
FN 9: IF +0 EQU +0 GOTO LBL 60
LBL 63
; ECKE 4
Q1 = Q1 - Q40
Q2 = Q2 - Q40
LBL 60
;Division durch 0 vermeiden
Q15 = ABS Q45
FN 10: IF +Q15 NE +90 GOTO LBL 30
Q45 = 89.9999 * SGN Q45
LBL 30
Q15 = ABS Q46
FN 10: IF +Q15 NE +90 GOTO LBL 31
Q46 = 89.9999 * SGN Q46
LBL 31
; ** GERADENGLEICHUNGEN
Q36 = ( ( Q1 / COS Q45 ) - ( Q2 / COS Q46 ) ) / ( TAN Q45 - TAN Q46 )
FN 9: IF +Q12 EQU +Q14 GOTO LBL 200
Q37 = ( Q1 - Q36 * SIN Q45 ) / COS Q45
LBL 200
Q35 = Q36
FN 9: IF +Q12 EQU +Q14 GOTO LBL 201
Q36 = Q37
FN 9: IF +0 EQU +0 GOTO LBL 202
LBL 201
Q36 = - Q2
LBL 202
Q32 = Q35
Q31 = Q36
Q1 = Q47
Q151 = Q31
Q152 = Q32
FN 10: IF +Q113 NE +1 GOTO LBL 170
Q151 = Q31 / 25.4
Q152 = Q32 / 25.4
LBL 170
CALL PGM PRESET.CYC
FN 10: IF +Q304 NE +1 GOTO LBL 199
FN 11: IF +Q305 GT +0 GOTO LBL 252
LBL 251
FN 18: SYSREAD Q0 = ID210 NR1
Q47 = Q0 + Q1
FN 17: SYSWRITE ID 210 NR1 =+Q47
FN 9: IF +Q303 EQU +0 GOTO LBL 199
FN 17: SYSWRITE ID 504 NRQ305 =+Q47
FN 9: IF +0 EQU +0 GOTO LBL 199
LBL 252
FN 9: IF +Q303 EQU +0 GOTO LBL 251
FN 18: SYSREAD Q0 = ID210 NR1
Q47 = Q0 + Q1
FN 17: SYSWRITE ID 504 NRQ305 =+Q47
LBL 199
FN 17: SYSWRITE ID 990 NR1 =+0
FN 17: SYSWRITE ID 990 NR2 =+0
FN 9: IF +Q381 EQU +0 GOTO LBL 254
TCH PROBE 417 BZPKT TS.-ACHSE ~
Q263=+Q382 ;1. PUNKT 1. ACHSE ~
Q264=+Q383 ;1. PUNKT 2. ACHSE ~
Q294=+Q384 ;1. PUNKT 3. ACHSE ~
Q320=+QL0 ;SICHERHEITS-ABST. ~
Q260=+Q260 ;SICHERE HOEHE ~
Q305=+Q305 ;NR. IN TABELLE ~
Q333=+Q333 ;BEZUGSPUNKT ~
Q303=+Q303 ;MESSWERT-UEBERGABE
LBL 254
Q199 = 0
END PGM 414 MM
