0  BEGIN PGM 400 MM
1  FN 18: SYSREAD Q0 = ID2000 NR10 IDX4158
2  FN 9: IF +Q0 EQU +1 GOTO LBL 254
3  FN 18: SYSREAD Q0 = ID990 NR2 IDX16
4  FN 9: IF +Q0 EQU +0 GOTO LBL 254
5  Q320 = ABS Q320  
6  Q272 = ABS Q272  
7  Q301 = ABS Q301  
8  FN 10: IF +Q109 NE -1 GOTO LBL 100
9  FN 14: ERROR = 1001
10  LBL 100
11  FN 18: SYSREAD Q0 = ID210 NR3
12  FN 9: IF +Q0 EQU +0 GOTO LBL 101
13  FN 14: ERROR = 1008
14  LBL 101
15  FN 18: SYSREAD Q0 = ID210 NR4 IDX1
16  FN 18: SYSREAD Q1 = ID210 NR4 IDX2
17  FN 18: SYSREAD Q2 = ID210 NR4 IDX3
18  Q0 = Q0 + Q1 + Q2  
19  FN 9: IF +Q0 EQU +3 GOTO LBL 102
20  FN 14: ERROR = 1007
21  LBL 102
22  FN 18: SYSREAD Q0 = ID210 NR6
23  FN 10: IF +Q0 NE -1 GOTO LBL 103
24  FN 14: ERROR = 1006
25  LBL 103
26  FN 17: SYSWRITE ID212 = +3
27  FN 17: SYSWRITE ID990 NR1 = +1
28  FN 17: SYSWRITE ID990 NR2 = +1
29  FN 17: SYSWRITE ID210 NR1 = +0
30  FN 18: SYSREAD Q3 = ID1000 NR6140
31  FN 18: SYSREAD Q1 = ID1010 NR6200
32  FN 9: IF +Q1 EQU +0 GOTO LBL 13
33  FN 18: SYSREAD Q1 = ID1000 NR6200
34  FN 9: IF +Q1 EQU +1 GOTO LBL 10
35  LBL 13
36  FN 18: SYSREAD Q58 = ID1000 NR6120
37  FN 18: SYSREAD Q59 = ID1000 NR6150
38  FN 18: SYSREAD Q2 = ID350 NR11
39  FN 9: IF +0 EQU +0 GOTO LBL 11
40  LBL 10
41  FN 18: SYSREAD Q58 = ID1000 NR6360
42  FN 18: SYSREAD Q59 = ID1000 NR6361
43  FN 18: SYSREAD Q2 = ID350 NR31
44  LBL 11
45  FN 18: SYSREAD Q0 = ID1000 NR7411
46  ; Q0 = 2 * FRAC (Q0 / 2)  
47  FN 9: IF +Q0 EQU +0 GOTO LBL 14
48  FN 18: SYSREAD Q2 = ID200 NR1
49  Q2 = ABS Q2  
50  LBL 14
51  FN 10: IF +Q0 NE +0 GOTO LBL 15
52  FN 17: SYSWRITE ID990 NR3 = +1
53  LBL 15
54  Q320 = Q320 + Q2 + Q3  
55  FN 10: IF +Q267 NE +0 GOTO LBL 12
56  FN 14: ERROR = 1042
57  LBL 12
58  ; ** Y-RICHTUNG ************** 
59  FN 12: IF +Q261 LT +Q260 GOTO LBL 130
60  FN 0: Q260 = +Q261
61  LBL 130
62  FN 1: Q1 = +Q109 + +1
63  FN 18: SYSREAD Q0 = ID270 NR1 IDXQ1
64  FN 11: IF +Q0 GT +Q260 GOTO LBL 120
65  L Z+Q260 R0 FQ59
66  LBL 120
67  FN 9: IF +Q272 EQU +1 GOTO LBL 21
68  FN 9: IF +Q267 EQU -1 GOTO LBL 20
69  FN 2: Q22 = +Q264 - +Q320
70  FN 2: Q32 = +Q266 - +Q320
71  L X+Q263 Y+Q22 R0 FQ59
72  TCH PROBE 0.0 BEZUGSEBENE Q4 Y+
73  TCH PROBE 0.1 Z+Q261
74  FN 9: IF +Q301 EQU +0 GOTO LBL 1
75  L Z+Q260 FQ59
76  LBL 1
77  L X+Q265 Y+Q32 FQ59
78  TCH PROBE 0.0 BEZUGSEBENE Q5 Y+
79  TCH PROBE 0.1 Z+Q261
80  FN 9: IF +0 EQU +0 GOTO LBL 92
81  LBL 20
82  FN 1: Q22 = +Q264 + +Q320
83  FN 1: Q32 = +Q266 + +Q320
84  L X+Q263 Y+Q22 R0 FQ59
85  TCH PROBE 0.0 BEZUGSEBENE Q4 Y-
86  TCH PROBE 0.1 Z+Q261
87  FN 9: IF +Q301 EQU +0 GOTO LBL 2
88  L Z+Q260 FQ59
89  LBL 2
90  L X+Q265 Y+Q32 FQ59
91  TCH PROBE 0.0 BEZUGSEBENE Q5 Y-
92  TCH PROBE 0.1 Z+Q261
93  FN 9: IF +0 EQU +0 GOTO LBL 92
94  LBL 21
95  ; ** X-RICHTUNG *************** 
96  FN 9: IF +Q267 EQU -1 GOTO LBL 22
97  FN 2: Q21 = +Q263 - +Q320
98  FN 2: Q31 = +Q265 - +Q320
99  L X+Q21 Y+Q264 R0 FQ59
100  TCH PROBE 0.0 BEZUGSEBENE Q4 X+
101  TCH PROBE 0.1 Z+Q261
102  FN 9: IF +Q301 EQU +0 GOTO LBL 3
103  L Z+Q260 FQ59
104  LBL 3
105  L X+Q31 Y+Q266 FQ59
106  TCH PROBE 0.0 BEZUGSEBENE Q5 X+
107  TCH PROBE 0.1 Z+Q261
108  FN 9: IF +0 EQU +0 GOTO LBL 91
109  LBL 22
110  FN 1: Q21 = +Q263 + +Q320
111  FN 1: Q31 = +Q265 + +Q320
112  L X+Q21 Y+Q264 R0 FQ59
113  TCH PROBE 0.0 BEZUGSEBENE Q4 X-
114  TCH PROBE 0.1 Z+Q261
115  FN 9: IF +Q301 EQU +0 GOTO LBL 4
116  L Z+Q260 FQ59
117  LBL 4
118  L X+Q31 Y+Q266 FQ59
119  TCH PROBE 0.0 BEZUGSEBENE Q5 X-
120  TCH PROBE 0.1 Z+Q261
121  FN 9: IF +0 EQU +0 GOTO LBL 91
122  ; Z-RICHTUNG 
123  LBL 91
124  Q50 = ATAN ((Q264 - Q266) / (Q4 - Q5))  
125  Q50 = Q50 - Q307  
126  CALL LBL 30
127  FN 9: IF +0 EQU +0 GOTO LBL 99
128  LBL 92
129  Q50 = ATAN ((Q5 - Q4) / (Q265 - Q263))  
130  Q50 = Q50 - Q307  
131  CALL LBL 30
132  FN 9: IF +0 EQU +0 GOTO LBL 99
133  LBL 30
134  FN 11: IF +Q50 GT +0 GOTO LBL 31
135  FN 11: IF +Q50 GT -360.0001 GOTO LBL 32
136  Q50 = Q50 + 360  
137  FN 9: IF +0 EQU +0 GOTO LBL 32
138  LBL 31
139  FN 12: IF +Q50 LT +360.0001 GOTO LBL 32
140  Q50 = Q50 - 360  
141  LBL 32
142  FN 11: IF +Q50 GT +0 GOTO LBL 40
143  FN 11: IF +Q50 GT -90.0001 GOTO LBL 98
144  FN 11: IF +Q50 GT -270.0001 GOTO LBL 33
145  Q50 = Q50 + 360  
146  FN 9: IF +0 EQU +0 GOTO LBL 98
147  LBL 33
148  Q50 = Q50 + 180  
149  FN 9: IF +0 EQU +0 GOTO LBL 98
150  LBL 40
151  FN 12: IF +Q50 LT +90.0001 GOTO LBL 98
152  FN 12: IF +Q50 LT +270.0001 GOTO LBL 34
153  Q50 = Q50 - 360  
154  FN 9: IF +0 EQU +0 GOTO LBL 98
155  LBL 34
156  Q50 = Q50 - 180  
157  LBL 98
158  LBL 0
159  LBL 99
160  L Z+Q260 R0 FQ59
161  FN 17: SYSWRITE ID210 NR1 = +Q50
162  FN 17: SYSWRITE ID990 NR1 = +0
163  FN 17: SYSWRITE ID990 NR2 = +0
164  LBL 254
165  END PGM 400 MM
