BEGIN PGM 419 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_CYC419.CYC
Q198 = 419
FN 10: IF +Q303 NE +1 GOTO LBL 10
CALL PGM INIT41X.CYC
LBL 10
CALL PGM INIT.CYC
;** Tastkugelradius umladen
Q4 = Q2
FN 10: IF +Q267 NE +0 GOTO LBL 1
FN 14: ERROR= 1042
LBL 1
;** Nur WZ-Achse X, Y, Z erlaubt
FN 12: IF +Q109 LT +3 GOTO LBL 2
FN 14: ERROR= 1079
LBL 2
Q320 = Q320 + Q2 + Q3
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
; Aktiven Preset lesen alle Achsen
FN 18: SYSREAD Q0 = ID530 NR1
FN 10: IF +Q0 NE -1 GOTO LBL 3
Q0 = 0
LBL 3
;Abfrage ob Anzeige setzen
FN 10: IF +Q305 NE +0 GOTO LBL 4
CALL LBL 60
LBL 4
FN 9: IF +Q272 EQU +1 GOTO LBL 21
FN 9: IF +Q272 EQU +2 GOTO LBL 31
FN 9: IF +Q272 EQU +3 GOTO LBL 41
FN 14: ERROR= 1064
; Antasten X-Achse (Hauptachse)
LBL 21
Q1 = Q263 + ( - Q320 * Q267 )
Q2 = 0
FN 9: IF +Q267 EQU -1 GOTO LBL 22
Q2 = Q41
FN 9: IF +0 EQU +0 GOTO LBL 23
LBL 22
Q2 = 180
Q2 = Q41 + 180
LBL 23
L  X+Q1  Y+Q264 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q2
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 10: IF +Q113 NE +1 GOTO LBL 162
Q143 = Q143 - ( Q114 * 25.4 )
FN 9: IF +0 EQU +0 GOTO LBL 163
LBL 162
Q143 = Q143 - Q114
LBL 163
FN 9: IF +0 EQU +0 GOTO LBL 98
;Y-Achse (Nebenachse)
LBL 31
Q1 = Q264 + ( - Q320 * Q267 )
Q2 = 90
FN 9: IF +Q267 EQU -1 GOTO LBL 32
Q2 = Q41 + Q2
FN 9: IF +0 EQU +0 GOTO LBL 33
LBL 32
Q2 = Q41 + 270
LBL 33
L  X+Q263  Y+Q1 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 X WINKEL:+Q2
TCH PROBE 1.2  Z+Q261
CALL PGM SUB_POSREAD.CYC
FN 10: IF +Q113 NE +1 GOTO LBL 164
Q143 = Q143 - ( Q114 * 25.4 )
FN 9: IF +0 EQU +0 GOTO LBL 165
LBL 164
Q143 = Q143 - Q114
LBL 165
FN 9: IF +0 EQU +0 GOTO LBL 98
LBL 41
;Z-Achse (Werkzeugachse)
Q1 = Q261 + ( - Q320 * Q267 )
Q2 = 0
FN 9: IF +Q267 EQU -1 GOTO LBL 42
FN 9: IF +Q272 EQU +3 GOTO LBL 43
Q2 = Q41
FN 9: IF +0 EQU +0 GOTO LBL 43
LBL 42
Q2 = 180
FN 9: IF +Q272 EQU +3 GOTO LBL 43
Q2 = Q41 + 180
LBL 43
L  X+Q263  Y+Q264 R0 FQ59
TCH PROBE 1.0 BEZUGSPUNKT POLAR
TCH PROBE 1.1 Z WINKEL:+Q2
TCH PROBE 1.2  Z+Q1
CALL PGM SUB_POSREAD.CYC
FN 10: IF +Q113 NE +1 GOTO LBL 160
Q143 = Q143 - ( Q114 * 25.4 )
FN 9: IF +0 EQU +0 GOTO LBL 161
LBL 160
Q143 = Q143 - Q114
LBL 161
LBL 98
L  Z+Q260 FQ59
FN 17: SYSWRITE ID 990 NR1 =+0
FN 17: SYSWRITE ID 990 NR2 =+0
;Drehungen lesen
FN 18: SYSREAD Q7 = ID210 NR1 ;ROTMAN
FN 18: SYSREAD Q8 = ID210 NR2 ;ROTPROG
FN 18: SYSREAD Q9 = ID210 NR1 ;3DROT
Q7 = Q7 + Q8 + Q9
;Werte schreiben abhaengig von WZ-Achse
FN 9: IF +Q109 EQU +0 GOTO LBL 51
FN 9: IF +Q109 EQU +1 GOTO LBL 71
FN 9: IF +Q109 EQU +2 GOTO LBL 91
LBL 51
;*************************************
;*************************************
;Toolachse = X-Achse
;Schreiben in Tabellen
FN 9: IF +Q272 EQU +1 GOTO LBL 156
FN 9: IF +Q272 EQU +2 GOTO LBL 157
FN 9: IF +Q272 EQU +3 GOTO LBL 158
;************* Y-Achse ***************
LBL 156
FN 9: IF +Q303 EQU +0 GOTO LBL 59
;** Write into Preset table
CALL LBL 56
FN 17: SYSWRITE ID 502 NRQ305 IDX2 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX3 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
;** Write into Datum table
LBL 59
CALL LBL 50
Q141 = Q141 - Q333 + ( Q267 * Q4 )
Q142 = Q142 - Q264
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
;************* Z-Achse ***************
LBL 157
FN 9: IF +Q303 EQU +0 GOTO LBL 69
;** Write into Preset table
CALL LBL 66
FN 17: SYSWRITE ID 502 NRQ305 IDX3 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX2 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
;** Write into Datum table
LBL 69
CALL LBL 50
Q141 = Q141 - Q263
Q142 = Q142 - Q333 + ( Q267 * Q4 )
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
;************* X-Achse ***************
LBL 158
Q143 = Q143 - Q333
FN 9: IF +Q303 EQU +0 GOTO LBL 68
;** Write into Preset table
FN 17: SYSWRITE ID 502 NRQ305 IDX1 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
LBL 68
;** Write into Datum table
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
;*************************************
;*************************************
LBL 71
;Toolachse = Y-Achse
;*************************************
;Schreiben in Tabellen
FN 9: IF +Q272 EQU +1 GOTO LBL 176
FN 9: IF +Q272 EQU +2 GOTO LBL 177
FN 9: IF +Q272 EQU +3 GOTO LBL 178
;Antastachse Z-Achse ***************
LBL 176
FN 9: IF +Q303 EQU +0 GOTO LBL 79
;** Write into Preset table
CALL LBL 56
FN 17: SYSWRITE ID 502 NRQ305 IDX3 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX1 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
LBL 79
;** Write into Datum table
CALL LBL 50
Q141 = Q141 - Q333 + ( Q267 * Q4 )
Q142 = Q142 - Q264
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
;Antastachse X ***************
LBL 177
FN 9: IF +Q303 EQU +0 GOTO LBL 89
;** Write into Preset table
CALL LBL 66
FN 17: SYSWRITE ID 502 NRQ305 IDX1 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX3 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
LBL 89
;** Write into Datum table
CALL LBL 50
Q141 = Q141 - Q263
Q142 = Q142 - Q333 + ( Q267 * Q4 )
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
; Antastachse Y ***************
LBL 178
Q143 = Q143 - Q333
FN 9: IF +Q303 EQU +0 GOTO LBL 88
;** Write into Preset table
FN 17: SYSWRITE ID 502 NRQ305 IDX2 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
LBL 88
;** Write into Datum table
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
;*************************************
;*************************************
LBL 91
;Toolachse = Z-Achse
;*************************************
;Schreiben in Tabellen
FN 9: IF +Q272 EQU +1 GOTO LBL 196
FN 9: IF +Q272 EQU +2 GOTO LBL 197
FN 9: IF +Q272 EQU +3 GOTO LBL 198
;Antastachse X ***************
LBL 196
FN 9: IF +Q303 EQU +0 GOTO LBL 99
;** Write into Preset table
CALL LBL 56
FN 17: SYSWRITE ID 502 NRQ305 IDX1 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX2 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
;** Write into Datum table
LBL 99
CALL LBL 50
Q141 = Q141 - Q333 + ( Q267 * Q4 )
Q142 = Q142 - Q264
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q141
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q142
FN 9: IF +0 EQU +0 GOTO LBL 254
;Antastachse Y ***************
LBL 197
FN 9: IF +Q303 EQU +0 GOTO LBL 109
;** Write into Preset table
CALL LBL 66
FN 17: SYSWRITE ID 502 NRQ305 IDX2 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 502 NRQ305 IDX1 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
;** Write into Datum table
LBL 109
CALL LBL 50
Q141 = Q141 - Q263
Q142 = Q142 - Q333 + ( Q267 * Q4 )
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q142
FN 9: IF +Q7 EQU +0 GOTO LBL 254
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q141
FN 9: IF +0 EQU +0 GOTO LBL 254
;Antastachse Z ***************
LBL 198
Q143 = Q143 - Q333
FN 9: IF +Q303 EQU +0 GOTO LBL 119
;** Write into Preset table
FN 17: SYSWRITE ID 502 NRQ305 IDX3 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
LBL 119
;** Write into Datum table
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q143
FN 9: IF +0 EQU +0 GOTO LBL 254
;**************************************************
;** Unterprogramme
;** Umrechnung fuer Schreiben NPT
LBL 50
;Schraeglagenkorrektur fr NPV
Q44 = ( Q50 * COS Q41 ) + ( Q51 * SIN Q41 )
Q45 = - ( Q50 * SIN Q41 ) + ( Q51 * COS Q41 )
Q141 = Q141 + Q44
Q142 = Q142 + Q45
LBL 0
;** Umrechnung fuer Schreiben PRT, X-Achse
LBL 56
; Neuer Preset ungedreht
Q48 = ( Q333 * COS - Q41 ) + ( Q142 * SIN - Q41 )
Q49 = - ( Q333 * SIN - Q41 ) + ( Q142 * COS - Q41 )
; Messwerte ungedreht
Q46 = ( Q141 * COS - Q41 ) + ( Q142 * SIN - Q41 )
Q47 = - ( Q141 * SIN - Q41 ) + ( Q142 * COS - Q41 )
; Korigierte Messwerte
Q141 = Q46 - Q48
Q142 = Q47 - Q49
;Kugelradius verrechnen
Q141 = Q141 + ( Q267 * Q4 * ( COS Q41 ) )
Q142 = Q142 + ( Q267 * Q4 * ( SIN Q41 ) )
LBL 0
;** Umrechnung fuer Schreiben PRT, Y-Achse
LBL 66
Q48 = ( Q141 * COS - Q41 ) + ( Q333 * SIN - Q41 )
Q49 = - ( Q141 * SIN - Q41 ) + ( Q333 * COS - Q41 )
Q46 = ( Q141 * COS - Q41 ) + ( Q142 * SIN - Q41 )
Q47 = - ( Q141 * SIN - Q41 ) + ( Q142 * COS - Q41 )
Q141 = Q46 - Q48
Q142 = Q47 - Q49
;Kugelradius verrechnen
Q141 = Q141 - ( Q267 * ( Q4 * SIN Q41 ) )
Q142 = Q142 + ( Q267 * ( Q4 * COS Q41 ) )
LBL 0
;** COPY ACTIVE LINE INTO LINE 0
LBL 60
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 254
FN 10: IF +Q305 NE +0 GOTO LBL 200
; Zeile 0 aktivieren
FN 17: SYSWRITE ID 530 NR1 =+Q305
LBL 200
END PGM 419 MM 
