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