|
1 /* |
|
2 |
|
3 Template C code used to produce target Ethercat C CIA402 code |
|
4 |
|
5 Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT |
|
6 |
|
7 Distributed under the terms of the GNU Lesser General Public License as |
|
8 published by the Free Software Foundation; either version 2 of the License, or |
|
9 (at your option) any later version. |
|
10 |
|
11 See COPYING file for copyrights details. |
|
12 |
|
13 */ |
|
14 |
|
15 #include "ecrt.h" |
|
16 |
|
17 #include "beremiz.h" |
|
18 #include "iec_types_all.h" |
|
19 |
|
20 #include "accessor.h" |
|
21 #include "POUS.h" |
|
22 |
|
23 /* From CiA402, page 27 |
|
24 |
|
25 Table 30 - State coding |
|
26 Statusword | PDS FSA state |
|
27 xxxx xxxx x0xx 0000 | Not ready to switch on |
|
28 xxxx xxxx x1xx 0000 | Switch on disabled |
|
29 xxxx xxxx x01x 0001 | Ready to switch on |
|
30 xxxx xxxx x01x 0011 | Switched on |
|
31 xxxx xxxx x01x 0111 | Operation enabled |
|
32 xxxx xxxx x00x 0111 | Quick stop active |
|
33 xxxx xxxx x0xx 1111 | Fault reaction active |
|
34 xxxx xxxx x0xx 1000 | Fault |
|
35 */ |
|
36 #define FSAFromStatusWord(SW) (SW & 0x006f) |
|
37 #define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000 |
|
38 #define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000 |
|
39 #define ReadyToSwitchOn 0b00100001 |
|
40 #define SwitchedOn 0b00100011 |
|
41 #define OperationEnabled 0b00100111 |
|
42 #define QuickStopActive 0b00000111 |
|
43 #define FaultReactionActive 0b00001111 FSA_sep 0b00101111 |
|
44 #define Fault 0b00001000 FSA_sep 0b00101000 |
|
45 |
|
46 // SatusWord bits : |
|
47 #define SW_ReadyToSwitchOn 0x0001 |
|
48 #define SW_SwitchedOn 0x0002 |
|
49 #define SW_OperationEnabled 0x0004 |
|
50 #define SW_Fault 0x0008 |
|
51 #define SW_VoltageEnabled 0x0010 |
|
52 #define SW_QuickStop 0x0020 |
|
53 #define SW_SwitchOnDisabled 0x0040 |
|
54 #define SW_Warning 0x0080 |
|
55 #define SW_Remote 0x0200 |
|
56 #define SW_TargetReached 0x0400 |
|
57 #define SW_InternalLimitActive 0x0800 |
|
58 |
|
59 // ControlWord bits : |
|
60 #define SwitchOn 0x0001 |
|
61 #define EnableVoltage 0x0002 |
|
62 #define QuickStop 0x0004 |
|
63 #define EnableOperation 0x0008 |
|
64 #define FaultReset 0x0080 |
|
65 #define Halt 0x0100 |
|
66 |
|
67 |
|
68 IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s; |
|
69 IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s; |
|
70 IEC_INT beremiz__IW%(location_str)s_402; |
|
71 IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402; |
|
72 |
|
73 %(MCL_headers)s |
|
74 |
|
75 static IEC_BOOL __FirstTick = 1; |
|
76 |
|
77 typedef struct { |
|
78 %(entry_variables)s |
|
79 axis_s* axis; |
|
80 } __CIA402Node; |
|
81 |
|
82 #define AxsPub __CIA402Node_%(location_str)s |
|
83 |
|
84 static __CIA402Node AxsPub; |
|
85 |
|
86 %(extern_located_variables_declaration)s |
|
87 |
|
88 %(fieldbus_interface_declaration)s |
|
89 |
|
90 int __init_%(location_str)s() |
|
91 { |
|
92 __FirstTick = 1; |
|
93 %(init_entry_variables)s |
|
94 *(AxsPub.ModesOfOperation) = 0x08; |
|
95 return 0; |
|
96 } |
|
97 |
|
98 void __cleanup_%(location_str)s() |
|
99 { |
|
100 } |
|
101 |
|
102 void __retrieve_%(location_str)s() |
|
103 { |
|
104 if (__FirstTick) { |
|
105 *__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF(); |
|
106 AxsPub.axis = |
|
107 __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402); |
|
108 AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s; |
|
109 %(init_axis_params)s |
|
110 %(fieldbus_interface_definition)s |
|
111 __FirstTick = 0; |
|
112 } |
|
113 |
|
114 // Default variables retrieve |
|
115 AxsPub.axis->CommunicationReady = |
|
116 *(AxsPub.StatusWord) != 0; |
|
117 #define FSA_sep || FSA == |
|
118 { |
|
119 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
|
120 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
|
121 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
|
122 } |
|
123 #undef FSA_sep |
|
124 AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition); |
|
125 AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity); |
|
126 AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque); |
|
127 |
|
128 // Extra variables retrieve |
|
129 %(extra_variables_retrieve)s |
|
130 } |
|
131 |
|
132 void __publish_%(location_str)s() |
|
133 { |
|
134 IEC_BOOL power = |
|
135 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
|
136 && AxsPub.axis->Power; |
|
137 uint16_t CW = *(AxsPub.ControlWord); |
|
138 |
|
139 #define FSA_sep : case |
|
140 // CIA402 node state transition computation |
|
141 switch (FSAFromStatusWord(*(AxsPub.StatusWord))) { |
|
142 case SwitchOnDisabled : |
|
143 CW &= ~(SwitchOn | FaultReset); |
|
144 CW |= EnableVoltage | QuickStop; |
|
145 break; |
|
146 case ReadyToSwitchOn : |
|
147 case OperationEnabled : |
|
148 if (!power) { |
|
149 CW &= ~(FaultReset | EnableOperation); |
|
150 CW |= SwitchOn | EnableVoltage | QuickStop; |
|
151 break; |
|
152 } |
|
153 case SwitchedOn : |
|
154 if (power) { |
|
155 CW &= ~(FaultReset); |
|
156 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
|
157 } |
|
158 break; |
|
159 case Fault : |
|
160 /* TODO reset fault only when MC_Reset */ |
|
161 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
162 CW |= FaultReset; |
|
163 break; |
|
164 default: |
|
165 break; |
|
166 } |
|
167 #undef FSA_sep |
|
168 *(AxsPub.ControlWord) = CW; |
|
169 |
|
170 // CIA402 node modes of operation computation according to axis motion mode |
|
171 switch (AxsPub.axis->AxisMotionMode) { |
|
172 case mc_mode_cst: |
|
173 *(AxsPub.ModesOfOperation) = 0x0a; |
|
174 break; |
|
175 case mc_mode_csv: |
|
176 *(AxsPub.ModesOfOperation) = 0x09; |
|
177 break; |
|
178 default: |
|
179 *(AxsPub.ModesOfOperation) = 0x08; |
|
180 break; |
|
181 } |
|
182 |
|
183 // Default variables publish |
|
184 *(AxsPub.TargetPosition) = |
|
185 AxsPub.axis->RawPositionSetPoint; |
|
186 *(AxsPub.TargetVelocity) = |
|
187 AxsPub.axis->RawVelocitySetPoint; |
|
188 *(AxsPub.TargetTorque) = |
|
189 AxsPub.axis->RawTorqueSetPoint; |
|
190 |
|
191 // Extra variables publish |
|
192 %(extra_variables_publish)s |
|
193 } |