BEGIN PGM PREST_41X MM
; Verwendet in Zyklus 410-416, 418
;*************************************
;Werte schreiben abhaengig von WZ-Achse
FN 9: IF +Q109 EQU +0 GOTO LBL "TOOLAX_X"
FN 9: IF +Q109 EQU +1 GOTO LBL "TOOLAX_Y"
FN 9: IF +Q109 EQU +2 GOTO LBL "TOOLAX_Z"
FN 9: IF +0 EQU +0 GOTO LBL "END"
LBL "TOOLAX_X"
;*************************************
;---------Toolachse = X-Achse---------
;*************************************
FN 9: IF +Q303 EQU +0 GOTO LBL "TOOLAX_X_WRITE_DATUM"
;=======Preset setzen==========
Q22 = Q261 ; Messhoehe
Q23 = Q31 ; Y
Q24 = Q32 ; Z
Q25 = Q331 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX2 = BLOCK Q22 - Q25
Q25 = Q332 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX3 = BLOCK Q22 - Q25
FN 9: IF +0 EQU +0 GOTO LBL "END"
;------------------------------
LBL "TOOLAX_X_WRITE_DATUM"
CALL LBL "ROT_DATUM_SHIFT"
Q31 = Q31 - Q331
Q32 = Q32 - Q332
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q31
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q32
FN 9: IF +0 EQU +0 GOTO LBL "END"
;**************************************************
LBL "TOOLAX_Y"
;*************************************
;---------Toolachse = Y-Achse---------
;*************************************
FN 9: IF +Q303 EQU +0 GOTO LBL "TOOLAX_Y_WRITE_DATUM"
;=======Preset setzen==========
Q22 = Q32 ; X
Q23 = Q261 ; Messhoehe
Q24 = Q31 ; Z
Q25 = Q331 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX3 = BLOCK Q22 - Q25
Q25 = Q332 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX1 = BLOCK Q22 - Q25
FN 9: IF +0 EQU +0 GOTO LBL "END"
;------------------------------
LBL "TOOLAX_Y_WRITE_DATUM"
CALL LBL "ROT_DATUM_SHIFT"
Q31 = Q31 - Q331
Q32 = Q32 - Q332
FN 17: SYSWRITE ID 500 NRQ305 IDX3 =+Q31
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q32
FN 9: IF +0 EQU +0 GOTO LBL "END"
;**************************************************
LBL "TOOLAX_Z"
;*************************************
;---------Toolachse = Z-Achse---------
;*************************************
FN 9: IF +Q303 EQU +0 GOTO LBL "TOOLAX_Z_WRITE_DATUM"
;=======Preset setzen==========
Q22 = Q31 ; X
Q23 = Q32 ; Y
Q24 = Q261 ; Messhoehe
Q25 = Q331 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX1 = BLOCK Q22 - Q25
Q25 = Q332 ; Bezugspunkt
FN 17: SYSWRITE ID 532 NRQ305 IDX2 = BLOCK Q22 - Q25
FN 9: IF +0 EQU +0 GOTO LBL "END"
;------------------------------
LBL "TOOLAX_Z_WRITE_DATUM"
CALL LBL "ROT_DATUM_SHIFT"
Q31 = Q31 - Q331
Q32 = Q32 - Q332
FN 17: SYSWRITE ID 500 NRQ305 IDX1 =+Q31
FN 17: SYSWRITE ID 500 NRQ305 IDX2 =+Q32
FN 9: IF +0 EQU +0 GOTO LBL "END"
;**************************************************
;** Unterprogramme **
;** Umrechnung fuer Schreiben NPT **
LBL "ROT_DATUM_SHIFT"
FN 18: SYSREAD QL0 = ID210 NR1;=>Grunddrehung BA Manuell
FN 18: SYSREAD QL1 = ID210 NR2;=>Drehung Zyklus 10
Q44 = ( Q50 * COS(  QL0 + QL1 ) ) + ( Q51 * SIN(  QL0 + QL1 ) )
Q45 = - ( Q50 * SIN(  QL0 + QL1 ) ) + ( Q51 * COS(  QL0 + QL1 ) )
Q31 = Q31 + Q44
Q32 = Q32 + Q45
LBL 0
;** Umrechnung fuer FN25, X-Achse **
LBL 55
Q48 = ( Q405 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q49 = - ( Q405 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q46 = ( Q31 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q47 = - ( Q31 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q31 = Q46
Q32 = Q47
LBL 0
;** Umrechnung fuer FN25, Y-Achse **
LBL 65
Q48 = ( Q31 * COS - Q41 ) + ( Q405 * SIN - Q41 )
Q49 = - ( Q31 * SIN - Q41 ) + ( Q405 * COS - Q41 )
Q46 = ( Q31 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q47 = - ( Q31 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q31 = Q46
Q32 = Q47
LBL 0
;** Umrechnung fuer Schreiben PRT, X-Achse **
LBL 56
Q48 = ( Q405 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q49 = - ( Q405 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q46 = ( Q31 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q47 = - ( Q31 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q31 = Q46 - Q48
Q32 = Q47 - Q49
LBL 0
;** Umrechnung fuer Schreiben PRT, Y-Achse **
LBL 66
Q48 = ( Q31 * COS - Q41 ) + ( Q405 * SIN - Q41 )
Q49 = - ( Q31 * SIN - Q41 ) + ( Q405 * COS - Q41 )
Q46 = ( Q31 * COS - Q41 ) + ( Q32 * SIN - Q41 )
Q47 = - ( Q31 * SIN - Q41 ) + ( Q32 * COS - Q41 )
Q31 = Q46 - Q48
Q32 = Q47 - Q49
LBL 0
LBL "END"
END PGM PREST_41X MM
