1 /* |
1 /* |
2 |
2 |
3 Template C code used to produce target Ethercat C CIA402 code |
3 Template C code used to produce target Ethercat C CIA402 code |
4 |
4 |
5 Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT |
5 Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT |
|
6 RTES Lab : CRKim, JBLee, youcu |
|
7 Higen Motor : Donggu Kang |
6 |
8 |
7 Distributed under the terms of the GNU Lesser General Public License as |
9 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 |
10 published by the Free Software Foundation; either version 2 of the License, or |
9 (at your option) any later version. |
11 (at your option) any later version. |
10 |
12 |
31 xxxx xxxx x01x 0111 | Operation enabled |
33 xxxx xxxx x01x 0111 | Operation enabled |
32 xxxx xxxx x00x 0111 | Quick stop active |
34 xxxx xxxx x00x 0111 | Quick stop active |
33 xxxx xxxx x0xx 1111 | Fault reaction active |
35 xxxx xxxx x0xx 1111 | Fault reaction active |
34 xxxx xxxx x0xx 1000 | Fault |
36 xxxx xxxx x0xx 1000 | Fault |
35 */ |
37 */ |
|
38 |
|
39 //ssh_add |
|
40 /* From CiA402, Page 63 Statusword for homing mode |
|
41 |
|
42 Table 106 - Definition of bit 10, bit 12, bit 13 |
|
43 |
|
44 xx00 x0xx xxxx xxxx | Homing procedure is in progress |
|
45 xx00 x1xx xxxx xxxx | Homing procedure is interrupted or not started |
|
46 xx01 x0xx xxxx xxxx | Homing is attained, but target is not reached |
|
47 xx01 x1xx xxxx xxxx | Homing procedure is completed successfully |
|
48 xx10 x0xx xxxx xxxx | Homing error occurred, velocity is not 0 |
|
49 xx10 x1xx xxxx xxxx | Homing error occurred, velocity is 0 |
|
50 xx11 xxxx xxxx xxxx | reserved |
|
51 */ |
|
52 |
36 #define FSAFromStatusWord(SW) (SW & 0x006f) |
53 #define FSAFromStatusWord(SW) (SW & 0x006f) |
|
54 //ssh_add |
|
55 #define HomingStatusWord(SW) (SW & 0x3400) |
|
56 #define FaultFromStatusWord(SW) (SW & 0x0008) |
37 #define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000 |
57 #define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000 |
38 #define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000 |
58 #define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000 |
39 #define ReadyToSwitchOn 0b00100001 |
59 #define ReadyToSwitchOn 0b00100001 |
40 #define SwitchedOn 0b00100011 |
60 #define SwitchedOn 0b00100011 |
41 #define OperationEnabled 0b00100111 |
61 #define OperationEnabled 0b00100111 |
42 #define QuickStopActive 0b00000111 |
62 #define QuickStopActive 0b00000111 |
43 #define FaultReactionActive 0b00001111 FSA_sep 0b00101111 |
63 #define FaultReactionActive 0b00001111 FSA_sep 0b00101111 |
44 #define Fault 0b00001000 FSA_sep 0b00101000 |
64 #define Fault 0b00001000 FSA_sep 0b00101000 |
45 |
65 |
46 // SatusWord bits : |
66 //ssh_add |
|
67 #define HomingInProgress 0b0000000000000000 |
|
68 #define HomingNotRunning 0b0000010000000000 |
|
69 #define HomingNotReached 0b0001000000000000 |
|
70 #define Homing_Completed 0b0001010000000000 |
|
71 #define HomingErrorInVelo 0b0010000000000000 |
|
72 #define HomingErrorNotVelo 0b0010010000000000 |
|
73 #define HomingReserved 0b0011000000000000 FSA_sep 0b0011010000000000 |
|
74 |
|
75 // StatusWord bits : |
47 #define SW_ReadyToSwitchOn 0x0001 |
76 #define SW_ReadyToSwitchOn 0x0001 |
48 #define SW_SwitchedOn 0x0002 |
77 #define SW_SwitchedOn 0x0002 |
49 #define SW_OperationEnabled 0x0004 |
78 #define SW_OperationEnabled 0x0004 |
50 #define SW_Fault 0x0008 |
79 #define SW_Fault 0x0008 |
51 #define SW_VoltageEnabled 0x0010 |
80 #define SW_VoltageEnabled 0x0010 |
54 #define SW_Warning 0x0080 |
83 #define SW_Warning 0x0080 |
55 #define SW_Remote 0x0200 |
84 #define SW_Remote 0x0200 |
56 #define SW_TargetReached 0x0400 |
85 #define SW_TargetReached 0x0400 |
57 #define SW_InternalLimitActive 0x0800 |
86 #define SW_InternalLimitActive 0x0800 |
58 |
87 |
|
88 //ssh_add |
|
89 #define SW_HomingAttained 0x1000 |
|
90 #define SW_HomingError 0x2000 |
|
91 |
59 // ControlWord bits : |
92 // ControlWord bits : |
60 #define SwitchOn 0x0001 |
93 #define SwitchOn 0x0001 |
61 #define EnableVoltage 0x0002 |
94 #define EnableVoltage 0x0002 |
62 #define QuickStop 0x0004 |
95 #define QuickStop 0x0004 |
63 #define EnableOperation 0x0008 |
96 #define EnableOperation 0x0008 |
64 #define FaultReset 0x0080 |
97 #define FaultReset 0x0080 |
65 #define Halt 0x0100 |
98 #define Halt 0x0100 |
66 |
99 |
67 |
100 //ssh_add |
68 IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s; |
101 //#define Homing_OperationStart 0x0010 |
69 IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s; |
102 #define Homing_OperationStart_Origin 0x0010 |
70 IEC_INT beremiz__IW%(location_str)s_402; |
103 #define Homing_OperationStart_Edit 0x001F |
71 IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402; |
104 |
|
105 IEC_INT beremiz__IW%(location)s = %(slave_pos)s; |
|
106 IEC_INT *__IW%(location)s = &beremiz__IW%(location)s; |
|
107 IEC_INT beremiz__IW%(location)s_402; |
|
108 IEC_INT *__IW%(location)s_402 = &beremiz__IW%(location)s_402; |
72 |
109 |
73 %(MCL_headers)s |
110 %(MCL_headers)s |
74 |
111 |
75 static IEC_BOOL __FirstTick = 1; |
112 static IEC_BOOL __FirstTick = 1; |
76 |
113 |
102 typedef struct { |
139 typedef struct { |
103 %(entry_variables)s |
140 %(entry_variables)s |
104 axis_s* axis; |
141 axis_s* axis; |
105 } __CIA402Node; |
142 } __CIA402Node; |
106 |
143 |
107 #define AxsPub __CIA402Node_%(location_str)s |
144 #define AxsPub __CIA402Node_%(location)s |
108 |
145 |
109 static __CIA402Node AxsPub; |
146 static __CIA402Node AxsPub; |
110 |
147 |
111 %(extern_located_variables_declaration)s |
148 %(extern_located_variables_declaration)s |
112 |
149 |
113 %(fieldbus_interface_declaration)s |
150 %(fieldbus_interface_declaration)s |
114 |
151 |
115 int __init_%(location_str)s() |
152 int __init_%(location)s() |
116 { |
153 { |
117 __FirstTick = 1; |
154 __FirstTick = 1; |
118 %(init_entry_variables)s |
155 %(init_entry_variables)s |
119 *(AxsPub.ModesOfOperation) = 0x08; |
|
120 return 0; |
156 return 0; |
121 } |
157 } |
122 |
158 |
123 void __cleanup_%(location_str)s() |
159 void __cleanup_%(location)s() |
124 { |
160 { |
125 } |
161 } |
126 |
162 |
127 void __retrieve_%(location_str)s() |
163 void __retrieve_%(location)s() |
128 { |
164 { |
129 if (__FirstTick) { |
165 if (__FirstTick) { |
130 *__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF(); |
166 *__IW%(location)s_402 = __MK_Alloc_AXIS_REF(); |
131 AxsPub.axis = |
167 AxsPub.axis = |
132 __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402); |
168 __MK_GetPublic_AXIS_REF(*__IW%(location)s_402); |
133 AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s; |
169 AxsPub.axis->NetworkPosition = beremiz__IW%(location)s; |
134 %(init_axis_params)s |
170 %(init_axis_params)s |
135 %(fieldbus_interface_definition)s |
171 %(fieldbus_interface_definition)s |
136 __FirstTick = 0; |
172 __FirstTick = 0; |
137 } |
173 } |
138 |
174 |
144 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
180 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
145 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
181 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
146 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
182 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
147 } |
183 } |
148 #undef FSA_sep |
184 #undef FSA_sep |
149 AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition); |
185 %(default_variables_retrieve)s |
150 AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity); |
|
151 AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque); |
|
152 |
186 |
153 // Extra variables retrieve |
187 // Extra variables retrieve |
154 %(extra_variables_retrieve)s |
188 %(extra_variables_retrieve)s |
155 } |
189 } |
156 |
190 |
157 void __publish_%(location_str)s() |
191 void __publish_%(location)s() |
158 { |
192 { |
159 IEC_BOOL power = |
193 IEC_BOOL power = |
160 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
194 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
161 && AxsPub.axis->Power; |
195 && AxsPub.axis->Power; |
162 uint16_t CW = *(AxsPub.ControlWord); |
196 uint16_t CW = *(AxsPub.ControlWord); |
179 if (power) { |
213 if (power) { |
180 CW &= ~(FaultReset); |
214 CW &= ~(FaultReset); |
181 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
215 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
182 } |
216 } |
183 break; |
217 break; |
184 case Fault : |
218 //ssh_check |
185 /* TODO reset fault only when MC_Reset */ |
219 // case Fault : |
186 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
220 // /* TODO reset fault only when MC_Reset */ |
187 CW |= FaultReset; |
221 // AxsPub.axis->DriveFault = 1; |
188 break; |
222 // CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
223 // CW |= FaultReset; |
|
224 // break; |
189 default: |
225 default: |
190 break; |
226 break; |
191 } |
227 } |
|
228 //ssh_add |
|
229 if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault) |
|
230 AxsPub.axis->DriveFault = 1; |
|
231 else{ |
|
232 AxsPub.axis->DriveFault = 0; |
|
233 AxsPub.axis->DriveFaultReset = 0; |
|
234 } |
|
235 if(AxsPub.axis->DriveFaultReset){ |
|
236 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
237 CW |= FaultReset; |
|
238 } |
|
239 |
|
240 //ssh_add |
|
241 switch (HomingStatusWord(*(AxsPub.StatusWord))) { |
|
242 case HomingInProgress: |
|
243 break; |
|
244 case HomingNotRunning: |
|
245 break; |
|
246 case HomingNotReached: |
|
247 break; |
|
248 case Homing_Completed: |
|
249 if(!AxsPub.axis->HomingCompleted) |
|
250 AxsPub.axis->HomingCompleted = 1; |
|
251 break; |
|
252 case HomingErrorInVelo: |
|
253 case HomingErrorNotVelo: |
|
254 if(!AxsPub.axis->HomingCompleted) |
|
255 AxsPub.axis->HomingCompleted = 1; |
|
256 break; |
|
257 case HomingReserved: |
|
258 break; |
|
259 } |
192 #undef FSA_sep |
260 #undef FSA_sep |
193 *(AxsPub.ControlWord) = CW; |
261 |
|
262 //ssh_add |
|
263 %(modeofop_homing_method)s |
|
264 |
|
265 *(AxsPub.ControlWord) = CW; |
|
266 |
194 |
267 |
195 // CIA402 node modes of operation computation according to axis motion mode |
268 // CIA402 node modes of operation computation according to axis motion mode |
196 switch (AxsPub.axis->AxisMotionMode) { |
269 %(modeofop_computation_mode)s |
197 case mc_mode_cst: |
|
198 *(AxsPub.ModesOfOperation) = 0x0a; |
|
199 break; |
|
200 case mc_mode_csv: |
|
201 *(AxsPub.ModesOfOperation) = 0x09; |
|
202 break; |
|
203 default: |
|
204 *(AxsPub.ModesOfOperation) = 0x08; |
|
205 break; |
|
206 } |
|
207 |
270 |
208 // Default variables publish |
271 // Default variables publish |
209 *(AxsPub.TargetPosition) = |
272 %(default_variables_publish)s |
210 AxsPub.axis->RawPositionSetPoint; |
|
211 *(AxsPub.TargetVelocity) = |
|
212 AxsPub.axis->RawVelocitySetPoint; |
|
213 *(AxsPub.TargetTorque) = |
|
214 AxsPub.axis->RawTorqueSetPoint; |
|
215 |
273 |
216 // Extra variables publish |
274 // Extra variables publish |
217 %(extra_variables_publish)s |
275 %(extra_variables_publish)s |
218 } |
276 } |