BEGIN PGM DEPTH MM 
CC IX+0 IY+0
;
;Q29 = WKZ-NR
;Q30 = LCUTS
;Q31 = ANGLE
;Q32 = MIN. VERFAHRWEG IX (2 * WKZ-R)
;Q33 = ZAEHLER ZUSTELLUNGEN
;Q34 = n ZUST.
;Q35 = n PENDELBEWEGUNGEN 
;Q36 = IZ
;Q37 = IX
;Q38 = TIEFE / PENDELBEWEGUNG (VOR UND ZURCK) (2 * Q36)
;Q39 = ZAEHLER PENDELBEWEGUNGEN (VOR UND ZURCK)
;Q40 = AKT. ZUSTELLTIEFE (Q40 = Q40 + Q41)
;Q41 = ZUSTELLWERT (Q202 bzw. Q201 + Q200)
;Q42 = TIEFE GES. (Q201 + Q200)
;Q43 = MAX. VERFAHRWEG IX 
;Q68 = 1. Seitenlnge 
;Q69 = 2. Seitenlnge 
;Q70 = Tiefe
;***********************************************
;****** BERECHNUNG ZUSTELLUNG / TIEFE **********
;***********************************************
;
FN 11: IF +Q202 GT +0 GOTO LBL 200
FN 14: ERROR= 1090 ;                            Fehler keine Zust. 
LBL 200
FN 18: SYSREAD Q29 = ID20 NR1 ;                 # WKZ-NR #
FN 18: SYSREAD Q30 = ID50 NR13 IDXQ29 ;         # LCUTS #
FN 18: SYSREAD Q31 = ID50 NR14 IDXQ29 ;         # ANGLE #
; 
CALL PGM 25x_error
Q18 = Q108 / Q4
;
Q40 = Q202 + Q200 ;                             ZUST. GES.
Q41 = Q40
Q42 = Q70 + Q200 ;                              TIEFE GES.
; Wenn Q215=1 (Schlichten): Tiefe -> Zust.
; nur Tiefe schlichten
FN 9: IF +Q64 EQU +0 GOTO LBL 25
; Boden schlichten -> keine Zust.
Q40 = Q369 + Q200 ;                             ZUST. GES.
Q41 = Q40
FN 9: IF +0 EQU +0 GOTO LBL 3
LBL 25
;--- Wenn Zust. > Tiefe, verwende Tiefe als Zust.
FN 11: IF +Q70 GT +Q202 GOTO LBL 22
Q33 = 0
Q40 = Q70 + Q200 ;                              ZUST. GES.
Q41 = Q40
Q34 = 0
FN 9: IF +0 EQU +0 GOTO LBL 3
LBL 22
;
;--- n: Anzahl Zust. -> Q34
Q34 = Q70 / Q202
Q0 = FRAC Q34
;--- Wenn gerader Wert, verwende diesen
FN 9: IF +Q0 EQU +0 GOTO LBL 21
Q34 = INT Q34
Q34 = Q34 + 1
LBL 21
Q33 = 1
FN 9: IF +0 EQU +0 GOTO LBL 3
;
;---Einsprung fuer berwachung Zustellung / Tiefe
LBL 1
FN 9: IF +Q64 EQU +1 GOTO LBL 99 ; wenn Boden Schlichten fertig
;---Letzte Zustellung: wenn Zaehler = nZust => Tiefe = Rest-
FN 9: IF +Q33 EQU +Q34 GOTO LBL 2
Q40 = Q40 + Q41 - Q200
FN 9: IF +0 EQU +0 GOTO LBL 3
LBL 2
Q40 = Q42 - Q40
Q41 = Q40 + Q200
LBL 3
;
;
;***********************************************
;
FN 9: IF +Q31 EQU +90 GOTO LBL "VERT"
FN 9: IF +Q366 EQU +0 GOTO LBL "VERT"
FN 9: IF +Q366 EQU +1 GOTO LBL "HELIC"
FN 9: IF +Q366 EQU +2 GOTO LBL "RECI"
; 
;***********************************************
;************ PENDELND EINTAUCHEN **************
;***********************************************
; 
; 
LBL "RECI"
;--- Platzverhaeltniss -> Q43
Q43 = Q68 / 2 - Q108 ; >>Q43 = Q68 - Q108<<     Halbe Laenge - WZ-R
FN 11: IF +Q68 GT +Q69 GOTO LBL 51
FN 9: IF +Q68 EQU +Q69 GOTO LBL 51
Q43 = Q69 / 2 - Q108 ; >>Q43 = Q69 - Q108<<     Halbe Laenge - WZ-R
LBL 51
Q43 = Q43 * 15 / 16 ;                           Faktor Wandung nicht verletzen
Q37 = Q43 ;                                     Verfahrweg X
Q36 = TAN Q31 * Q43 ;                           Pendeltiefe
Q38 = 2 * Q36
;
;>>Q17 = Q37 / 2 ; Erste Pendelbewegung X / 2<<
;>>Q16 = Q36 / 2 ; Erste Pendelbewegung Z / 2<<
;
;--- Wenn nur eine Pendelbewegung neue Tiefe u. Verfahrweg
FN 11: IF +Q41 GT +Q38 GOTO LBL 20
Q36 = Q41 / 2
Q37 = Q36 / TAN Q31
Q35 = 2
LBL 20
;
Q32 = Q108 * 2
;-Frheres Verhalten iTNC, wird vorerst nicht verwendet
;---- Wenn Pendellaenge kl. Wkz-D, verwende Wkz-D-
;-FN 11: IF +Q37 GT +Q32 GOTO LBL 23
;-Q37 = Q32
;-LBL 23
; 
;---Wenn Pendellaenge gr. Verfahrber. X -> Fehler
FN 11: IF +Q43 GT +Q32 GOTO LBL 24
;Fehler o. ueber mitte schneidendes WZ 
;ggf. in beide richtungen verfahren derzeit nur rechts
; >>   << = Vorbereitet fuer beidseitiges Eintauchen
;FN 14: ERROR= 1104 
LBL 24
; 
; 
;--- n - Anzahl Pendelbew. -> Q35
LBL 12
Q35 = Q41 / Q36 ; >>Q35 = ( Q41 - Q36 / 2 ) / Q36<<
Q35 = Q35 / 2
Q0 = FRAC Q35
;--- Wenn ungerader Wert + 1 
FN 9: IF +Q0 EQU +0 GOTO LBL 11
Q35 = INT Q35
Q35 = Q35 + 1
Q36 = Q41 / ( Q35 * 2 ) ;                        Neue Pendeltiefe
; >>Q36 = ( Q41 - Q16 ) / ( Q35 * 2 )<<
LBL 11
Q39 = 0
; 
;-----------------------------------------------
;------------------ Einpendeln -----------------
; >>L IX-Q17 IZ-Q16 R0 FQ206<< Y-Richtung fehlt noch
LBL 120
FN 11: IF +Q69 GT +Q68 GOTO LBL 121
L IX+Q37 IZ-Q36 R0 FQ206
L IX-Q37 IZ-Q36 R0 FQ206
LBL 121
FN 11: IF +Q68 GT +Q69 GOTO LBL 122
FN 9: IF +Q68 EQU +Q69 GOTO LBL 122
L IY+Q37 IZ-Q36 R0 FQ206
L IY-Q37 IZ-Q36 R0 FQ206
LBL 122
Q39 = Q39 + 1 ;                                 Zaehler Pendeln
FN 12: IF +Q39 LT +Q35 GOTO LBL 120
; >>L IX+Q17 R0 FQ206<< Y-Richtung fehlt noch
CALL LBL "CALLPGM"
L IZ+Q200
;
;--- Ruecksprung zu Berechnung Tiefenzust.
Q33 = Q33 + 1 ;                                 Zaehler Zustellung
;--- Wenn Zaehler ungleich o. gleich nZust
FN 9: IF +Q33 EQU +Q34 GOTO LBL 1
FN 12: IF +Q33 LT +Q34 GOTO LBL 1
FN 9: IF +0 EQU +0 GOTO LBL 99
; 
; 
;***********************************************
;************ Helikaes Eintauchen **************
;***********************************************
;
LBL "HELIC"
; 
;*** Helix Radius Q37 ***
Q43 = Q68 / 2 - 2 * Q18 ;                       Halbe Laenge - WZ-d
Q0 = Q69 / 2 - Q18 ;                            Kleinere Seitenl. 
FN 11: IF +Q68 GT +Q69 GOTO LBL 52
FN 9: IF +Q68 EQU +Q69 GOTO LBL 52
Q43 = Q69 / 2 - 2 * Q18 ;                       Halbe Laenge - WZ-d
Q0 = Q68 / 2 - Q18 ;                            Kleinere Seitenl. 
LBL 52
Q43 = Q43 * 15 / 16 ;                           Faktor Wandung nicht verletzen
Q37 = Q18 * 15 / 16 ;                           Radius Helix Max.
Q13 = Q37
FN 11: IF +Q0 GT +Q37 GOTO LBL 36
Q37 = Q0 * 15 / 16 ;                            R Helix Min.
LBL 36
Q44 = Q37 * PI ;                                Halber Umfang
Q36 = TAN Q31 * Q44 ;                           Pendeltiefe
Q38 = 2 * Q36
;
DECLARE STRING QS30
QS30 = SYSSTR( ID10025 NR1 )
DECLARE STRING QS31 = "CfgGeoCycle"
DECLARE STRING QS32 = "suppressPlungeErr"
Q12 = CFGREAD( KEY_QS30 TAG_QS31 ATR_QS32 )
FN 9: IF +Q12 EQU +1 GOTO LBL 155
FN 11: IF +Q0 GT +Q13 GOTO LBL 155
FN 14: ERROR= 1104
LBL 155
;
FN 9: IF +1 EQU +0 GOTO LBL 98
; 
;--- Wenn nur eine Pendelbewegung neue Tiefe u. Verfahrweg
FN 11: IF +Q41 GT +Q38 GOTO LBL 30
Q36 = Q41 / 2
; Q37 = ( Q36 / TAN Q31 ) / PI
Q35 = 2
LBL 30
; 
LBL 98
; 
;--- n - Anzahl Pendelbew. -> Q35
Q35 = Q41 / Q36
Q35 = Q35 / 2
Q0 = FRAC Q35
;--- Wenn ungerader Wert + 1 
FN 9: IF +Q0 EQU +0 GOTO LBL 31
Q35 = INT Q35
Q35 = Q35 + 1
Q36 = Q41 / ( Q35 * 2 ) ;                       Neue Pendeltiefe
LBL 31
Q39 = 0
; 
;-----------------------------------------------
;-------------------- Helix --------------------
;-----------------------------------------------
;
LP  PR+Q37  PA+0 R0 FQ206
LBL 32
CP IPA+180 IZ-Q36 DR+ FQ206
CP IPA+180 IZ-Q36 DR+ FQ206
Q39 = Q39 + 1 ;-Zaehler Pendeln
FN 12: IF +Q39 LT +Q35 GOTO LBL 32
LP  PR+0  PA+0 R0 FQ206
CALL LBL "CALLPGM"
L IZ+Q200
;
;
;--- Ruecksprung zu Berechnung Tiefenzust.
Q33 = Q33 + 1 ;-Zahler Zustellung
;--- Wenn Zaehler ungleich o. gleich nZust
FN 9: IF +Q33 EQU +Q34 GOTO LBL 1
FN 12: IF +Q33 LT +Q34 GOTO LBL 1
LP  PR+0  PA+0 R0 FQ206
FN 9: IF +0 EQU +0 GOTO LBL 99
; 
; 
;***********************************************
;****************** Senkrecht *****************
;***********************************************
;
; 
LBL "VERT"
L IZ-Q41 R0 FQ206
CALL LBL "CALLPGM"
L IZ+Q200
;
;
;--- Ruecksprung zu Berechnung Tiefenzust.
Q33 = Q33 + 1 ;-Zahler Zustellung
;--- Wenn Zaehler ungleich o. gleich nZust
FN 9: IF +Q33 EQU +Q34 GOTO LBL 1
FN 12: IF +Q33 LT +Q34 GOTO LBL 1
FN 9: IF +0 EQU +0 GOTO LBL 99
; 
;
;-----------------------------------------------
;--------Programm-Aufruf -> Ausraeumen----------
;-----------------------------------------------
LBL "CALLPGM"
FN 9: IF +Q3 EQU +251 GOTO LBL 251
FN 9: IF +Q3 EQU +252 GOTO LBL 252
FN 9: IF +0 EQU +0 GOTO LBL 299
LBL 251
CALL PGM 251_Mill
FN 9: IF +0 EQU +0 GOTO LBL 299
LBL 252
CALL PGM 252_Mill
LBL 299
LBL 0
;***********************************************
LBL 99
;
END PGM DEPTH MM 
