BEGIN PGM 414 MM
FN 9: IF Q305  EQU 0  GOTO LBL "IFNOT_1"
 FN 18: SYSREAD QL0 = ID 10 NR 103 IDX 303
 FN 9: IF QL0  EQU 0 GOTO LBL "IF_2"
 FN 9: IF Q303  EQU -1 GOTO LBL "IF_2"
 FN 9: IF 0 EQU 0 GOTO LBL "IFNOT_2"
 LBL "IF_2"
  FN 14: ERROR= 1078
 LBL "IFNOT_2"
LBL "IFNOT_1"
CALL PGM CHECK_BAMODE.CYC
FN 9: IF +Q169 EQU +1.1 GOTO LBL 254
Q198 = 414
CALL PGM ORIENT.CYC
FN 10: IF +Q303 NE +1 GOTO LBL 10
CALL PGM INIT41X.CYC
LBL 10
CALL PGM INIT.CYC
QL1 = Q320
;Q41 = CYC10 + BASROT
Q25 = 0 + Q41
Q26 = 90 + Q41
Q27 = 180 + Q41
Q28 = 270 + Q41
Q40 = Q2
Q320 = Q320 + Q40 + Q3
FN 0: Q267 =+1
FN 11: IF +Q297 GT +Q264 GOTO LBL 49
FN 0: Q267 =-1
LBL 49
FN 0: Q302 =+1
FN 11: IF +Q263 GT +Q296 GOTO LBL 51
FN 0: Q302 =-1
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
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
Q2 = Q264 - Q320
Q31 = Q263 + Q326
FN 9: IF +Q33 EQU +1 GOTO LBL 53
Q31 = Q263 - Q326
LBL 53
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
LBL 20
Q2 = Q264 + Q320
Q31 = Q263 + Q326
FN 9: IF +Q33 EQU +4 GOTO LBL 54
Q31 = Q263 - Q326
LBL 54
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
; ** 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_41X.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 +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
;Spindelachse Y?
FN 9: IF +Q109 EQU +1 GOTO LBL 91
;Antastachse Z?
FN 9: IF +Q109 EQU +2 GOTO LBL 92
Q23 = 4 ;### Achs-ID A=4, B=5, C=6
FN 9: IF +0 EQU +0 GOTO LBL 93
LBL 91
Q23 = 5 ;### Achs-ID A=4, B=5, C=6
FN 9: IF +0 EQU +0 GOTO LBL 93
LBL 92
Q23 = 6 ;### Achs-ID A=4, B=5, C=6
LBL 93
Q21 = Q305 ;### Preset-Nummer
Q22 = 1 ;### 0=Achs-Offset, 1=Basistrafo
Q25 = 0 ;### Preset nach Eintrag aktivieren, 0= Vorheriger Preset
Q24 = Q47
Q26 = 1 ; Schreibe in Zeile aus Q21
CYCL DEF 952
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=+QL1 ;SICHERHEITS-ABST. ~
Q260=+Q260 ;SICHERE HOEHE ~
Q305=+Q305 ;NR. IN TABELLE ~
Q333=+Q333 ;BEZUGSPUNKT ~
Q303=+Q303 ;MESSWERT-UEBERGABE
LBL 254
FN 17: SYSWRITE ID 990 NR16 =+0 ;# Spindelkontrolle aus
Q199 = 0
END PGM 414 MM
