BEGIN PGM 415 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 = 415
CALL PGM ORIENT.CYC
FN 10: IF +Q303 NE +1 GOTO LBL 10
CALL PGM INIT41X.CYC
LBL 10
CALL PGM INIT.CYC
Q29 = Q320
Q25 = 0 + Q41
Q26 = 90 + Q41
Q27 = 180 + Q41
Q28 = 270 + Q41
Q40 = Q2
Q320 = Q320 + Q40 + Q3
FN 0: Q267 =-1
FN 0: Q302 =-1
FN 9: IF +Q308 EQU +1 GOTO LBL 51
FN 0: Q267 =-1
FN 0: Q302 =+1
FN 9: IF +Q308 EQU +2 GOTO LBL 51
FN 0: Q267 =+1
FN 0: Q302 =+1
FN 9: IF +Q308 EQU +3 GOTO LBL 51
FN 0: Q267 =+1
FN 0: Q302 =-1
LBL 51
FN 0: Q33 =+Q308
FN 9: IF +Q33 EQU 4 GOTO LBL "CaseEdge_4"
FN 9: IF +Q33 EQU 3 GOTO LBL "CaseEdge_3"
FN 9: IF +Q33 EQU 2 GOTO LBL "CaseEdge_2"
    QL11 = Q263 + Q320  ; Sollposition X1
    QL12 = Q264 + Q320  ; Sollposition Y1
    QL13 = Q263 + Q326  ; Sollposition X2
    QL14 = QL12         ; Sollposition Y2
    QL16 = Q263 + Q320  ; Sollposition X3
    QL17 = Q264 + Q320  ; Sollposition Y3
    QL18 = QL16         ; Sollposition X4
    QL19 = Q264 + Q327  ; Sollposition Y4
    QL21 = Q28          ; Antastrichtung 1
    QL22 = Q27          ; Antastrichtung 2
FN 9: IF 0 EQU 0 GOTO LBL "CaseEdge_End"
LBL "CaseEdge_2"        ; rechts unten
    QL11 = Q263 - Q320  ; Sollposition X1
    QL12 = Q264 + Q320  ; Sollposition Y1
    QL13 = Q263 - Q326  ; Sollposition X2
    QL14 = QL12         ; Sollposition Y2
    QL16 = Q263 - Q320  ; Sollposition X3
    QL17 = Q264 + Q320  ; Sollposition Y3
    QL18 = QL16         ; Sollposition X4
    QL19 = Q264 + Q327  ; Sollposition Y4
    QL21 = Q28          ; Antastrichtung 1
    QL22 = Q25          ; Antastrichtung 2
FN 9: IF 0 EQU 0 GOTO LBL "CaseEdge_End"
LBL "CaseEdge_3"        ; rechts oben
    QL11 = Q263 - Q320  ; Sollposition X1
    QL12 = Q264 - Q320  ; Sollposition Y1
    QL13 = Q263 - Q326  ; Sollposition X2
    QL14 = QL12         ; Sollposition Y2
    QL16 = Q263 - Q320  ; Sollposition X3
    QL17 = Q264 - Q320  ; Sollposition Y3
    QL18 = QL16         ; Sollposition X4
    QL19 = Q264 - Q327  ; Sollposition Y4
    QL21 = Q26          ; Antastrichtung 1
    QL22 = Q25          ; Antastrichtung 2
FN 9: IF 0 EQU 0 GOTO LBL "CaseEdge_End"
LBL "CaseEdge_4"        ; links oben
    QL11 = Q263 + Q320  ; Sollposition X1
    QL12 = Q264 - Q320  ; Sollposition Y1
    QL13 = Q263 + Q326  ; Sollposition X2
    QL14 = QL12         ; Sollposition Y2
    QL16 = Q263 + Q320  ; Sollposition X3
    QL17 = Q264 - Q320  ; Sollposition Y3
    QL18 = QL16         ; Sollposition X4
    QL19 = Q264 - Q327  ; Sollposition Y4
    QL21 = Q26          ; Antastrichtung 1
    QL22 = Q27          ; Antastrichtung 2
LBL "CaseEdge_End"
; ** Y-RICHTUNG **************
FN 12: IF +Q261 LT +Q260 GOTO LBL 130
FN 0: Q260 =+Q261
FN 0: Q301 =+0
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
L  X+QL11  Y+QL12 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+QL21
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+QL13  Y+QL14 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+QL21
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q13 =+Q141
FN 0: Q14 =+Q142
FN 9: IF +Q301 EQU +0 GOTO LBL 23
L  Z+Q260 FQ59
LBL 23
; ** X-RICHTUNG **********
L  X+QL16  Y+QL17 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+QL22
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+QL18  Y+QL19 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+QL22
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 0: Q18 =+Q141
FN 0: Q19 =+Q142
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
; ECKE 1
FN 11: IF +Q33 GT +1 GOTO LBL 61
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=+Q29 ;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 415 MM
