BEGIN PGM 25x_SlotPosition MM
*############################
*# Funktion fr Nutenzyklen
*# Berechne Position der Nut
*# 25.11.2008 L.Buchner
*# Argumente:
*#	- Q40			Zyklusnummer
*# Rckgaben:
*#	- Q80			1. Nut: Drehlage (Mittelpunktswinkel)
*#	- Q81			1. Nut: TeilkreisMitte X
*#	- Q82			1. Nut: TeilkreisMitte Y
*#	- Q83			1. Nut: Startwinkel
*#	- Q84			1. Nut: Endwinkel
*#	- Q85			Teilkreisradius
*############################

*############################
*# global belegte Parameter und Konstanten
*############################
; Q1					IST-Position Hauptachse
; Q2					IST-Position Nebenachse
*############################
*# Aktuelle Position
*############################
FN 10: IF +Q90 NE +0 GOTO LBL "TOOLAX_X"
	FN 18: SYSREAD Q1 = ID270 NR1 IDX2				; Hauptachse
	FN 18: SYSREAD Q2 = ID270 NR1 IDX3				; Nebenachse
LBL "TOOLAX_X"
FN 10: IF +Q90 NE +1 GOTO LBL "TOOLAX_Y"
	FN 18: SYSREAD Q1 = ID270 NR1 IDX3				; Hauptachse
	FN 18: SYSREAD Q2 = ID270 NR1 IDX1				; Nebenachse
LBL "TOOLAX_Y"
FN 10: IF +Q90 NE +2 GOTO LBL "TOOLAX_Z"
	FN 18: SYSREAD Q1 = ID270 NR1 IDX1				; Hauptachse
	FN 18: SYSREAD Q2 = ID270 NR1 IDX2				; Nebenachse
LBL "TOOLAX_Z"
*############################
*# Nut: Zyklus 253
*############################
FN 10: IF +Q40 NE +253 GOTO LBL "POS253_END"
	
	Q80 = Q374																; Drehlage
	Q19 = Q219																; Lnge
	Q18 = Q218																; Breite
	FN 12: IF +Q219 LT +Q218 GOTO LBL "253_REL_WID_LEN"
		Q80 = Q80 + 90													; Rotation um 90 wenn Breite grer Lnge
		Q18 = Q219															; Lnge
		Q19 = Q218															; Breite
	LBL "253_REL_WID_LEN"	
	Q83 = ( Q80 + 180 ) % 360									; Startwinkel
	Q84 = Q80																	; Endwinkel
	Q85 = ( Q18 - Q19 ) / 2										; Teilkreisradius
	
	FN 9: IF +Q367 EQU +1 GOTO LBL "POS253_START"
	FN 9: IF +Q367 EQU +2 GOTO LBL "POS253_STARTCENTRE"
	FN 9: IF +Q367 EQU +3 GOTO LBL "POS253_ENDCENTRE"
	FN 9: IF +Q367 EQU +4 GOTO LBL "POS253_ENDPOINT"
		Q81 = Q1																			; X-TeilkreisMitte
		Q82 = Q2																			; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS253_END"
	LBL "POS253_START"
		Q81 = Q1 + ( Q18 / 2 ) * COS Q80							; X-TeilkreisMitte
		Q82 = Q2 + ( Q18 / 2 ) * SIN Q80							; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS253_END"
	LBL "POS253_STARTCENTRE"
		Q81 = Q1 + ( (Q18-Q19) / 2 ) * COS Q80			; X-TeilkreisMitte
		Q82 = Q2 + ( (Q18-Q19) / 2 ) * SIN Q80			; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS253_END"		
	LBL "POS253_ENDPOINT"
		Q81 = Q1 + ( Q18 / 2 ) * COS(Q80+180)				; X-TeilkreisMitte
		Q82 = Q2 + ( Q18 / 2 ) * SIN(Q80+180)				; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS253_END"
	LBL "POS253_ENDCENTRE"
		Q81 = Q1 + ( (Q18-Q19) / 2 ) * COS(Q80+180)	; X-TeilkreisMitte
		Q82 = Q2 + ( (Q18-Q19) / 2 ) * SIN(Q80+180)	; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS253_END"		

LBL "POS253_END"
*############################

*############################
*# Runde Nut: Zyklus 254
*############################
FN 10: IF +Q40 NE +254 GOTO LBL "POS254_END"

	Q85 = Q375 / 2														; Teilkreisradius
	
	FN 9: IF +Q367 EQU +3 GOTO LBL "POS254_INITSTART"
	FN 9: IF +Q367 EQU +2 GOTO LBL "POS254_INITMIDDLE"
	FN 9: IF +Q367 EQU +1 GOTO LBL "POS254_INITEND"
	
		Q80 = Q376 + (Q248 / 2)									; Drehlage
		Q83 = Q80 - (Q248 / 2)									; Startwinkel
		Q84 = Q80 + (Q248 / 2)									; Endwinkel
		Q81 = Q216															; X-TeilkreisMitte
		Q82 = Q217															; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS254_END"
	LBL "POS254_INITSTART"
		Q80 = Q376 + (Q248 / 2)									; Drehlage
		Q83 = Q80 - (Q248 / 2)									; Startwinkel
		Q84 = Q80 + (Q248 / 2)									; Endwinkel
		Q81 = Q1 + Q85 * COS( 180 + Q83 )				; X-TeilkreisMitte	
		Q82 = Q2 + Q85 * SIN( 180 + Q83 )				; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS254_END"
	LBL "POS254_INITMIDDLE"
		Q80 = Q376; // + (Q70 * Q378)								; Drehlage
		Q83 = Q80 - (Q248 / 2)									; Startwinkel
		Q84 = Q80 + (Q248 / 2)									; Endwinkel
		Q81 = Q1 + Q85 * COS( 180 + Q80 ); X-TeilkreisMitte	
		Q82 = Q2 + Q85 * SIN( 180 + Q80 ); Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS254_END"
	LBL "POS254_INITEND"
		Q80 = Q376 - (Q248 / 2)									; Drehlage
		Q83 = Q80 - (Q248 / 2)									; Startwinkel
		Q84 = Q80 + (Q248 / 2)									; Endwinkel
		Q81 = Q1 + Q85 * COS( 180 + Q84 )				; X-TeilkreisMitte	
		Q82 = Q2 + Q85 * SIN( 180 + Q84 )				; Y-TeilkreisMitte
		FN 9: IF +0 EQU +0 GOTO LBL "POS254_END"

LBL "POS254_END"
*############################

END PGM MM
