0 BEGIN PGM 40201_NL MM 
1 ;Programma voor baanberekening en -beweging
2 ;voor de bewerking van een 3D-rechtlijnig
3 ;vlak over vijf assen.
4 FUNCTION TCPM F CONT AXIS SPAT PATHCTRL VECTOR
5 L  A+Q1016  B+Q1013  C+0 R0 FQ28 M3
6 L  Z+Q25 R0 FQ28
7 CALL LBL 1
8 CALL LBL 2
9 FN 9: IF +0 EQU +0 GOTO LBL 20
10 LBL 1 ;STARTWAARDEN
11 FN 0: Q30 =+0
12 FN 4: Q46 =+Q26 DIV +Q29
13 FN 0: Q31 =+Q1
14 FN 0: Q32 =+Q2
15 FN 1: Q33 =+Q3 + +Q23
16 FN 0: Q34 =+Q4
17 FN 0: Q35 =+Q5
18 FN 1: Q36 =+Q6 + +Q23
19 FN 1: Q37 =+Q33 + +Q24
20 FN 1: Q38 =+Q36 + +Q24
21 FN 0: Q39 =+Q1013
22 FN 1: Q61 =+Q3 + +Q23
23 FN 1: Q65 =+Q61 + +Q24
24 FN 1: Q62 =+Q6 + +Q23
25 FN 1: Q66 =+Q62 + +Q24
26 FN 1: Q63 =+Q9 + +Q23
27 FN 1: Q67 =+Q63 + +Q24
28 FN 1: Q64 =+Q12 + +Q23
29 FN 1: Q68 =+Q64 + +Q24
30 LBL 0
31 LBL 2 ;DELTAS
32 FN 2: Q51 =+Q10 - +Q1
33 FN 4: Q51 =+Q51 DIV +Q46
34 FN 2: Q52 =+Q11 - +Q2
35 FN 4: Q52 =+Q52 DIV +Q46
36 FN 2: Q53 =+Q12 - +Q3
37 FN 4: Q53 =+Q53 DIV +Q46
38 FN 2: Q54 =+Q7 - +Q4
39 FN 4: Q54 =+Q54 DIV +Q46
40 FN 2: Q55 =+Q8 - +Q5
41 FN 4: Q55 =+Q55 DIV +Q46
42 FN 2: Q56 =+Q9 - +Q6
43 FN 4: Q56 =+Q56 DIV +Q46
44 FN 0: Q43 =+Q1013
45 FN 12: IF +Q1013 LT +180 GOTO LBL 13
46 FN 2: Q43 =+Q1013 - +360
47 LBL 13
48 FN 0: Q45 =+Q1015
49 FN 12: IF +Q1015 LT +180 GOTO LBL 15
50 FN 2: Q45 =+Q1015 - +360
51 LBL 15
52 FN 2: Q50 =+Q45 - +Q43
53 FN 4: Q50 =+Q50 DIV +Q46
54 FN 0: Q40 =+Q21
55 FN 0: Q41 =+Q21
56 FN 8: Q47 =+Q54 LEN +Q55
57 FN 8: Q42 =+Q51 LEN +Q52
58 FN 1: Q42 =+Q42 + +Q47
59 FN 4: Q42 =+Q108 DIV +Q42
60 FN 3: Q42 =+Q42 * +4
61 FN 3: Q42 =+Q21 * +Q42
62 FN 4: Q42 =+Q42 DIV +2
63 FN 0: Q43 =+Q21
64 LBL 0
65 LBL 3 ;UPDATE
66 FN 1: Q30 =+Q30 + +Q29
67 FN 1: Q31 =+Q31 + +Q51
68 FN 1: Q32 =+Q32 + +Q52
69 FN 1: Q33 =+Q33 + +Q53
70 FN 1: Q34 =+Q34 + +Q54
71 FN 1: Q35 =+Q35 + +Q55
72 FN 1: Q36 =+Q36 + +Q56
73 FN 1: Q37 =+Q33 + +Q108
74 FN 1: Q37 =+Q37 + +Q24
75 FN 1: Q38 =+Q36 + +Q108
76 FN 1: Q38 =+Q38 + +Q24
77 FN 0: Q40 =+Q42
78 FN 1: Q39 =+Q39 + +Q50
79 LBL 0
80 LBL 20
81 L  X+Q1  Y+Q2  A+Q1016  B+Q1013 R0 FQ28
82 L  Z+Q25 R0 FQ42
83 L  X+Q31  Y+Q32  Z+Q33 R0 FQ20
84 L  X+Q34  Y+Q35  Z+Q36  A+Q1014  B+Q1013 R0 FQ21
85 L  X+Q34  Y+Q35  Z+Q38 R0 FQ20
86 L  X+Q31  Y+Q32  Z+Q37  A+Q1016  B+Q1013 R0 FQ21
87 LBL 10
88 CALL LBL 3
89 L  X+Q31  Y+Q32  Z+Q33  A+Q1016  B+Q39 R0 FQ22
90 L  X+Q34  Y+Q35  Z+Q36  A+Q1014  B+Q39 R0 FQ40
91 FN 9: IF +Q30 EQU +Q26 GOTO LBL 99
92 FN 11: IF +Q30 GT +Q26 GOTO LBL 99
93 L
94 L  X+Q34  Y+Q35  Z+Q38  A+Q1014  B+Q39 R0 FQ27
95 L  X+Q31  Y+Q32  Z+Q37  A+Q1016  B+Q39 R0 FQ28
96 L  X+Q31  Y+Q32  Z+Q33  A+Q1016  B+Q39 R0 FQ42
97 FN 9: IF +Q30 EQU +Q26 GOTO LBL 10
98 FN 12: IF +Q30 LT +Q26 GOTO LBL 10
99 LBL 99
100 L  Z+Q25  A+0  B+0  C+0 R0 FQ28
101 FUNCTION RESET TCPM
102 END PGM 40201_NL MM