0  BEGIN PGM M331 MM 
1  ****************
2  * - Description
3  ****************
4  * 1. Read nominal AxesAngle plus Rotation
5  * 2. Calculates the arriveable AxesAngles
6  * 3. Overwrite AxesAngle (Q120-Q122) and activate TCPM
7  ****************
8  * - Read HIRTH-Step
9  ****************
10 FN 17: SYSWRITE ID 2000 NR70 IDX7098 =+331 ; NUMBER NC MACRO ON
11 Q0 = 0 ; Start logic Axis
12 Q20 = Q120 ; Default Position A
13 Q21 = Q121 ; Default Position B
14 Q22 = Q122 ; Default Position C
15 Q40 = 0 ; Default Step A
16 Q41 = 0 ; Default Step B
17 Q42 = 0 ; Default Step C
18 LBL "READ_HIRTH_STEP"
19 FN 18: SYSREAD Q1 = ID1010 NR420 IDXQ0 ; MP-Index existing
20 FN 10: IF +Q1 NE +1 GOTO LBL "READ_HIRTH_STEP_END"
21 Q0 = Q0 + 1 ; Add Counter
22 FN 11: IF +Q0 GT +14 GOTO LBL "READ_HIRTH_STEP_END"
23 FN 18: SYSREAD Q1 = ID20 NR15 IDXQ0 ; logic Axis
24 FN 9: IF +Q1 EQU +3 GOTO LBL "READ_HIRTH_STEP_A"
25 FN 9: IF +Q1 EQU +4 GOTO LBL "READ_HIRTH_STEP_B"
26 FN 9: IF +Q1 EQU +5 GOTO LBL "READ_HIRTH_STEP_C"
27 FN 9: IF +0 EQU +0 GOTO LBL "READ_HIRTH_STEP"
28 LBL "READ_HIRTH_STEP_A"
29 Q2 = Q0 - 1 ; MP-Index
30 FN 18: SYSREAD Q10 = ID1000 NR420 IDXQ2 ; HIRTH active
31 FN 10: IF +Q10 NE +1 GOTO LBL "READ_HIRTH_STEP"
32 FN 18: SYSREAD Q40 = ID1000 NR430 IDXQ2 ; HIRTH Step A
33 FN 9: IF +0 EQU +0 GOTO LBL "READ_HIRTH_STEP"
34 LBL "READ_HIRTH_STEP_B"
35 Q2 = Q0 - 1 ; MP-Index
36 FN 18: SYSREAD Q11 = ID1000 NR420 IDXQ2 ; HIRTH active
37 FN 10: IF +Q11 NE +1 GOTO LBL "READ_HIRTH_STEP"
38 FN 18: SYSREAD Q41 = ID1000 NR430 IDXQ2 ; HIRTH Step B
39 FN 9: IF +0 EQU +0 GOTO LBL "READ_HIRTH_STEP"
40 LBL "READ_HIRTH_STEP_C"
41 Q2 = Q0 - 1 ; MP-Index
42 FN 18: SYSREAD Q12 = ID1000 NR420 IDXQ2 ; HIRTH active
43 FN 10: IF +Q12 NE +1 GOTO LBL "READ_HIRTH_STEP"
44 FN 18: SYSREAD Q42 = ID1000 NR430 IDXQ2 ; HIRTH Step C
45 FN 9: IF +0 EQU +0 GOTO LBL "READ_HIRTH_STEP"
46 LBL "READ_HIRTH_STEP_END"
47 FN 18: SYSREAD Q43 = ID210 NR8 IDX0 ; Rotation
48 ****************
49 * - Round AxesAngles
50 ****************
51 *  - Hirth Position A-Axis
52 FN 9: IF +Q10 EQU +0 GOTO LBL "A_HIRTHSTEP_END"
53 Q1 = 0
54 Q2 = INT ( Q120 / Q40 )
55 Q3 = ABS FRAC ( Q120 / Q40 )
56 FN 12: IF +Q3 LT +0.5 GOTO LBL "A_HIRTHSTEP_OK"
57 Q1 = Q40 * SGN Q120
58 LBL "A_HIRTHSTEP_OK"
59 Q20 = ( Q2 * Q40 + Q1 ) % 360
60 LBL "A_HIRTHSTEP_END"
61 *  - Hirth Position B-Axis
62 FN 9: IF +Q11 EQU +0 GOTO LBL "B_HIRTHSTEP_END"
63 Q1 = 0
64 Q2 = INT ( Q121 / Q41 )
65 Q3 = ABS FRAC ( Q121 / Q41 )
66 FN 12: IF +Q3 LT +0.5 GOTO LBL "B_HIRTHSTEP_OK"
67 Q1 = Q41 * SGN Q121
68 LBL "B_HIRTHSTEP_OK"
69 Q21 = ( Q2 * Q41 + Q1 ) % 360
70 LBL "B_HIRTHSTEP_END"
71 *  - Hirth Position C-Axis
72 FN 9: IF +Q12 EQU +0 GOTO LBL "C_HIRTHSTEP_END"
73 Q1 = 0
74 Q2 = INT ( Q122 / Q42 )
75 Q3 = ABS FRAC ( Q122 / Q42 )
76 FN 12: IF +Q3 LT +0.5 GOTO LBL "C_HIRTHSTEP_OK"
77 Q1 = Q42 * SGN Q122
78 LBL "C_HIRTHSTEP_OK"
79 Q22 = ( Q2 * Q42 + Q1 ) % 360
80 LBL "C_HIRTHSTEP_END"
81 ****************
82 * - TCPM
83 ****************
84 FN 17: SYSWRITE ID 210 NR6 =+0 ; 3D-ROT Auto inactive
85 FUNCTION TCPM F TCP AXIS POS PATHCTRL AXIS ; M128
86 FN 17: SYSWRITE ID 210 NR6 =-1 ; 3D-ROT Auto active
87 ; [...] !
88 Q120 = Q20 ; A-AxisPos
89 Q121 = Q21 ; B-AxisPos
90 Q122 = Q22 ; C-AxisPos
91 ****************
92 * - Trafos global
93 ****************
94 FN 17: SYSWRITE ID 420 NR0 =+0
95 ****************
96 FN 17: SYSWRITE ID 2000 NR70 IDX7098 =-1 ; NC MACRO OFF
97 END PGM M331 MM