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 |
77 typedef struct { |
114 typedef struct { |
78 %(entry_variables)s |
115 %(entry_variables)s |
79 axis_s* axis; |
116 axis_s* axis; |
80 } __CIA402Node; |
117 } __CIA402Node; |
81 |
118 |
82 #define AxsPub __CIA402Node_%(location_str)s |
119 #define AxsPub __CIA402Node_%(location)s |
83 |
120 |
84 static __CIA402Node AxsPub; |
121 static __CIA402Node AxsPub; |
85 |
122 |
86 %(extern_located_variables_declaration)s |
123 %(extern_located_variables_declaration)s |
87 |
124 |
88 %(fieldbus_interface_declaration)s |
125 %(fieldbus_interface_declaration)s |
89 |
126 |
90 int __init_%(location_str)s() |
127 int __init_%(location)s() |
91 { |
128 { |
92 __FirstTick = 1; |
129 __FirstTick = 1; |
93 %(init_entry_variables)s |
130 %(init_entry_variables)s |
94 *(AxsPub.ModesOfOperation) = 0x08; |
|
95 return 0; |
131 return 0; |
96 } |
132 } |
97 |
133 |
98 void __cleanup_%(location_str)s() |
134 void __cleanup_%(location)s() |
99 { |
135 { |
100 } |
136 } |
101 |
137 |
102 void __retrieve_%(location_str)s() |
138 void __retrieve_%(location)s() |
103 { |
139 { |
104 if (__FirstTick) { |
140 if (__FirstTick) { |
105 *__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF(); |
141 *__IW%(location)s_402 = __MK_Alloc_AXIS_REF(); |
106 AxsPub.axis = |
142 AxsPub.axis = |
107 __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402); |
143 __MK_GetPublic_AXIS_REF(*__IW%(location)s_402); |
108 AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s; |
144 AxsPub.axis->NetworkPosition = beremiz__IW%(location)s; |
109 %(init_axis_params)s |
145 %(init_axis_params)s |
110 %(fieldbus_interface_definition)s |
146 %(fieldbus_interface_definition)s |
111 __FirstTick = 0; |
147 __FirstTick = 0; |
112 } |
148 } |
113 |
149 |
119 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
155 uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord)); |
120 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
156 AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn; |
121 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
157 AxsPub.axis->PowerFeedback = FSA == OperationEnabled; |
122 } |
158 } |
123 #undef FSA_sep |
159 #undef FSA_sep |
124 AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition); |
160 %(default_variables_retrieve)s |
125 AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity); |
|
126 AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque); |
|
127 |
161 |
128 // Extra variables retrieve |
162 // Extra variables retrieve |
129 %(extra_variables_retrieve)s |
163 %(extra_variables_retrieve)s |
130 } |
164 } |
131 |
165 |
132 void __publish_%(location_str)s() |
166 void __publish_%(location)s() |
133 { |
167 { |
134 IEC_BOOL power = |
168 IEC_BOOL power = |
135 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
169 ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) |
136 && AxsPub.axis->Power; |
170 && AxsPub.axis->Power; |
137 uint16_t CW = *(AxsPub.ControlWord); |
171 uint16_t CW = *(AxsPub.ControlWord); |
154 if (power) { |
188 if (power) { |
155 CW &= ~(FaultReset); |
189 CW &= ~(FaultReset); |
156 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
190 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation; |
157 } |
191 } |
158 break; |
192 break; |
159 case Fault : |
193 //ssh_check |
160 /* TODO reset fault only when MC_Reset */ |
194 // case Fault : |
161 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
195 // /* TODO reset fault only when MC_Reset */ |
162 CW |= FaultReset; |
196 // AxsPub.axis->DriveFault = 1; |
163 break; |
197 // CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
198 // CW |= FaultReset; |
|
199 // break; |
164 default: |
200 default: |
165 break; |
201 break; |
166 } |
202 } |
|
203 //ssh_add |
|
204 if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault) |
|
205 AxsPub.axis->DriveFault = 1; |
|
206 else{ |
|
207 AxsPub.axis->DriveFault = 0; |
|
208 AxsPub.axis->DriveFaultReset = 0; |
|
209 } |
|
210 if(AxsPub.axis->DriveFaultReset){ |
|
211 CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation); |
|
212 CW |= FaultReset; |
|
213 } |
|
214 |
|
215 //ssh_add |
|
216 switch (HomingStatusWord(*(AxsPub.StatusWord))) { |
|
217 case HomingInProgress: |
|
218 break; |
|
219 case HomingNotRunning: |
|
220 break; |
|
221 case HomingNotReached: |
|
222 break; |
|
223 case Homing_Completed: |
|
224 if(!AxsPub.axis->HomingCompleted) |
|
225 AxsPub.axis->HomingCompleted = 1; |
|
226 break; |
|
227 case HomingErrorInVelo: |
|
228 case HomingErrorNotVelo: |
|
229 if(!AxsPub.axis->HomingCompleted) |
|
230 AxsPub.axis->HomingCompleted = 1; |
|
231 break; |
|
232 case HomingReserved: |
|
233 break; |
|
234 } |
167 #undef FSA_sep |
235 #undef FSA_sep |
168 *(AxsPub.ControlWord) = CW; |
236 |
|
237 //ssh_add |
|
238 %(modeofop_homing_method)s |
|
239 |
|
240 *(AxsPub.ControlWord) = CW; |
|
241 |
169 |
242 |
170 // CIA402 node modes of operation computation according to axis motion mode |
243 // CIA402 node modes of operation computation according to axis motion mode |
171 switch (AxsPub.axis->AxisMotionMode) { |
244 %(modeofop_computation_mode)s |
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 |
245 |
183 // Default variables publish |
246 // Default variables publish |
184 *(AxsPub.TargetPosition) = |
247 %(default_variables_publish)s |
185 AxsPub.axis->RawPositionSetPoint; |
|
186 *(AxsPub.TargetVelocity) = |
|
187 AxsPub.axis->RawVelocitySetPoint; |
|
188 *(AxsPub.TargetTorque) = |
|
189 AxsPub.axis->RawTorqueSetPoint; |
|
190 |
248 |
191 // Extra variables publish |
249 // Extra variables publish |
192 %(extra_variables_publish)s |
250 %(extra_variables_publish)s |
193 } |
251 } |