0 BEGIN PGM 8170_nl MM 
1 ;Bepalen van het middelpunt van
2 ;een boring met drie meetpunten
3 ;en referentiepunt X/Y in het
4 ;middelpunt instellen.
5 ;Voor iTNC530 en TNC426
6 ;vanaf SW-nr. 280 472-XX
7 ;
8 BLK FORM 0.1 Z  X+0  Y+0  Z-50
9 BLK FORM 0.2  X+50  Y+50  Z+0
10 ;
11 ;Gereedschapsoproep taster
12 TOOL CALL 501 Z
13 ;
14 ;Invoer parameters
15 FN 0: Q30 =+25 ;MIDDELPUNT VAN DE BORING OP X-AS
16 FN 0: Q31 =+25 ;MIDDELPUNT VAN DE BORING OP Y-AS
17 FN 0: Q32 =-5 ;MEETHOOGTE! MIDDELPUNT VAN DE TASTKOGEL!
18 FN 0: Q33 =+40 ;BORINGSDIAMETER
19 FN 0: Q34 =+0 ;HOEK 1E TASTPUNT! POSITIEF!
20 FN 0: Q35 =+120 ;HOEK 2E TASTPUNT! POSITIEF!
21 FN 0: Q36 =+240 ;HOEK 3E TASTPUNT! POSITIEF!
22 FN 0: Q37 =+5 ;EXTRA VEILIGHEIDSAFSTAND IN TASTRICHTING !additief TOT MP6140!
23 FN 0: Q38 =+20 ;VEILIGE HOOGTE! ABSOLUUT!
24 FN 0: Q60 =+0 ;NIEUW REFERENTIEPUNT X-AS
25 FN 0: Q61 =+0 ;NIEUW REFERENTIEPUNT Y-AS
26 ;Einde invoer!
27 ;
28 ;Beslissing of 1e hoek moet worden omgerekend
29 FN 12: IF +Q34 LT +180 GOTO LBL 1
30 FN 9: IF +Q34 EQU +180 GOTO LBL 1
31 FN 11: IF +Q34 GT +0 GOTO LBL 1
32 FN 9: IF +Q34 EQU +0 GOTO LBL 1
33 FN 1: Q34 =-360 + +Q34
34 ;
35 ;Berekeningen
36 LBL 1
37 FN 18: SYSREAD Q39 = ID1000 NR6140
38 FN 3: Q37 =+Q37 * +Q37
39 FN 5: Q37 = SQRT +Q37
40 FN 2: Q40 =+Q32 - +Q108
41 FN 4: Q41 =+Q33 DIV +2
42 FN 2: Q42 =+Q41 - +Q37
43 FN 2: Q43 =+Q42 - +Q39
44 FN 2: Q44 =+Q43 - +Q108
45 ;
46 ;Berekening positie 1e tastpunt
47 FN 7: Q50 = COS +Q34
48 FN 3: Q51 =+Q50 * +Q42
49 FN 1: Q52 =+Q51 + +Q30
50 FN 6: Q53 = SIN +Q34
51 FN 3: Q54 =+Q53 * +Q42
52 FN 1: Q55 =+Q54 + +Q31
53 FN 3: Q56 =+Q50 * +Q44
54 FN 1: Q57 =+Q56 + +Q30
55 FN 3: Q58 =+Q53 * +Q44
56 FN 1: Q59 =+Q54 + +Q31
57 L  Z+Q38 R0 FMAX
58 L  X+Q57  Y+Q59 FMAX
59 L  Z+2 FMAX
60 L  Z+Q40 F2000
61 ;1e tastcyclus
62 TCH PROBE 1.0 BEZUGSPUNKT POLAR
63 TCH PROBE 1.1 X WINKEL:+Q34
64 TCH PROBE 1.2  X+Q52  Y+Q55  Z+Q32
65 FN 0: Q90 =+Q115
66 FN 0: Q91 =+Q116
67 L  Z+Q38 FMAX
68 ;
69 ;Beslissing of 2e hoek moet worden omgerekend
70 FN 12: IF +Q35 LT +180 GOTO LBL 2
71 FN 9: IF +Q35 EQU +180 GOTO LBL 2
72 FN 11: IF +Q35 GT +0 GOTO LBL 2
73 FN 9: IF +Q35 EQU +0 GOTO LBL 2
74 FN 1: Q35 =-360 + +Q35
75 ;
76 ;Berekening positie 2e tastpunt
77 LBL 2
78 FN 7: Q50 = COS +Q35
79 FN 3: Q51 =+Q50 * +Q42
80 FN 1: Q52 =+Q51 + +Q30
81 FN 6: Q53 = SIN +Q35
82 FN 3: Q54 =+Q53 * +Q42
83 FN 1: Q55 =+Q54 + +Q31
84 FN 3: Q56 =+Q50 * +Q44
85 FN 1: Q57 =+Q56 + +Q30
86 FN 3: Q58 =+Q53 * +Q44
87 FN 1: Q59 =+Q58 + +Q31
88 L  Z+Q38 R0 FMAX
89 L  X+Q57  Y+Q59 FMAX
90 L  Z+2 FMAX
91 L  Z+Q40 F2000
92 ;2e tastcyclus
93 TCH PROBE 1.0 BEZUGSPUNKT POLAR
94 TCH PROBE 1.1 X WINKEL:+Q35
95 TCH PROBE 1.2  X+Q52  Y+Q55  Z+Q32
96 FN 0: Q92 =+Q115
97 FN 0: Q93 =+Q116
98 L  Z+Q38 FMAX
99 ;
100 ;Beslissing of 3e hoek moet worden omgerekend
101 FN 12: IF +Q36 LT +180 GOTO LBL 3
102 FN 9: IF +Q36 EQU +180 GOTO LBL 3
103 FN 11: IF +Q36 GT +0 GOTO LBL 3
104 FN 9: IF +Q36 EQU +0 GOTO LBL 3
105 FN 1: Q36 =-360 + +Q36
106 ;
107 ;Berekening positie 3e tastpunt
108 LBL 3
109 FN 7: Q50 = COS +Q36
110 FN 3: Q51 =+Q50 * +Q42
111 FN 1: Q52 =+Q51 + +Q30
112 FN 6: Q53 = SIN +Q36
113 FN 3: Q54 =+Q53 * +Q42
114 FN 1: Q55 =+Q54 + +Q31
115 FN 3: Q56 =+Q50 * +Q44
116 FN 1: Q57 =+Q56 + +Q30
117 FN 3: Q58 =+Q53 * +Q44
118 FN 1: Q59 =+Q58 + +Q31
119 L  Z+Q38 R0 FMAX
120 L  X+Q57  Y+Q59 FMAX
121 L  Z+2 FMAX
122 L  Z+Q40 F2000
123 ;3e tastcyclus
124 TCH PROBE 1.0 BEZUGSPUNKT POLAR
125 TCH PROBE 1.1 X WINKEL:+Q36
126 TCH PROBE 1.2  X+Q52  Y+Q55  Z+Q32
127 FN 0: Q94 =+Q115
128 FN 0: Q95 =+Q116
129 L  Z+Q38 FMAX
130 ;
131 ;
132 ;Cirkelmiddelpunt bepalen
133 FN 23: Q97 = CDATA Q90
134 ;Referentiepunt X en Y instellen
135 FN 25: PRESET =  X /+Q97 /+Q60
136 FN 25: PRESET =  Y /+Q98 /+Q61
137 ;
138 ;Einde programma
139 M30
140 END PGM 8170_nl MM