29 } __DS402NodeState; |
33 } __DS402NodeState; |
30 |
34 |
31 typedef struct { |
35 typedef struct { |
32 %(entry_variables)s |
36 %(entry_variables)s |
33 __DS402NodeState state; |
37 __DS402NodeState state; |
|
38 axis_s* axis; |
34 } __DS402Node; |
39 } __DS402Node; |
35 |
40 |
36 static __DS402Node __DS402Node_%(location)s; |
41 static __DS402Node __DS402Node_%(location)s; |
37 |
42 |
38 %(located_variables_declaration)s |
43 %(extern_located_variables_declaration)s |
39 |
|
40 extern uint8_t *domain1_pd; |
|
41 %(extern_pdo_entry_configuration)s |
|
42 |
44 |
43 int __init_%(location)s() |
45 int __init_%(location)s() |
44 { |
46 { |
|
47 %(init_entry_variables)s; |
|
48 *__IW%(location)s_0 = __MK_AllocAxis(&(__DS402Node_%(location)s.axis)); |
45 return 0; |
49 return 0; |
46 } |
50 } |
47 |
51 |
48 void __cleanup_%(location)s() |
52 void __cleanup_%(location)s() |
49 { |
53 { |
50 } |
54 } |
51 |
55 |
52 void __retrieve_%(location)s() |
56 void __retrieve_%(location)s() |
53 { |
57 { |
54 IEC_UINT statusword_inactive = __DS402Node_%(location)s.StatusWord & __InactiveMask; |
58 IEC_UINT statusword_inactive = *(__DS402Node_%(location)s.StatusWord) & __InactiveMask; |
55 IEC_UINT statusword_active = __DS402Node_%(location)s.StatusWord & __ActiveMask; |
59 IEC_UINT statusword_active = *(__DS402Node_%(location)s.StatusWord) & __ActiveMask; |
56 |
|
57 // DS402 input entries extraction |
|
58 %(retrieve_variables)s |
|
59 |
60 |
60 // DS402 node state computation |
61 // DS402 node state computation |
61 __DS402Node_%(location)s.state = __Unknown; |
62 __DS402Node_%(location)s.state = __Unknown; |
62 switch (statusword_inactive) { |
63 switch (statusword_inactive) { |
63 case 0x00: |
64 case 0x00: |
93 } |
94 } |
94 if (__DS402Node_%(location)s.state == __Unknown) { |
95 if (__DS402Node_%(location)s.state == __Unknown) { |
95 return; |
96 return; |
96 } |
97 } |
97 |
98 |
|
99 __DS402Node_%(location)s.axis->PowerFeedback = __DS402Node_%(location)s.state != __OperationEnabled; |
|
100 __DS402Node_%(location)s.axis->ActualPosition = (IEC_REAL)(*(__DS402Node_%(location)s.ActualPosition)) * __DS402Node_%(location)s.axis->RatioDenominator / __DS402Node_%(location)s.axis->RatioNumerator; |
|
101 |
|
102 __MK_UpdateAxis(*__IW%(location)s_0); |
98 } |
103 } |
99 |
104 |
100 void __publish_%(location)s() |
105 void __publish_%(location)s() |
101 { |
106 { |
|
107 __MK_ComputeAxis(*__IW%(location)s_0); |
|
108 |
|
109 IEC_BOOL power = ((*(__DS402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __DS402Node_%(location)s.axis->Power; |
|
110 |
102 // DS402 node state transition computation |
111 // DS402 node state transition computation |
103 switch (__DS402Node_%(location)s.state) { |
112 switch (__DS402Node_%(location)s.state) { |
104 case __SwitchOnDisabled: |
113 case __SwitchOnDisabled: |
105 __DS402Node_%(location)s.ControlWord = (__DS402Node_%(location)s.ControlWord & ~0x87) | 0x06; |
114 *(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x87) | 0x06; |
106 break; |
115 break; |
107 case __ReadyToSwitchOn: |
116 case __ReadyToSwitchOn: |
108 __DS402Node_%(location)s.ControlWord = (__DS402Node_%(location)s.ControlWord & ~0x8f) | 0x07; |
117 case __OperationEnabled: |
109 break; |
118 if (!power) { |
|
119 *(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x07; |
|
120 break; |
|
121 } |
110 case __SwitchedOn: |
122 case __SwitchedOn: |
111 // if (POWER) { |
123 if (power) { |
112 // __DS402Node_%(location)s.ControlWord = (__DS402Node_%(location)s.ControlWord & ~0x8f) | 0x0f; |
124 *(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f; |
113 // } |
125 } |
114 break; |
|
115 case __OperationEnabled: |
|
116 // if (!POWER) { |
|
117 // __DS402Node_%(location)s.ControlWord = (__DS402Node_%(location)s.ControlWord & ~0x8f) | 0x07; |
|
118 // } |
|
119 break; |
126 break; |
120 case __Fault: |
127 case __Fault: |
121 __DS402Node_%(location)s.ControlWord = (__DS402Node_%(location)s.ControlWord & ~0x8f) | 0x80; |
128 *(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x80; |
122 break; |
129 break; |
123 default: |
130 default: |
124 break; |
131 break; |
125 } |
132 } |
126 |
133 |
127 // DS402 output entries setting |
134 *(__DS402Node_%(location)s.TargetPosition) = (IEC_DINT)(__DS402Node_%(location)s.axis->PositionSetPoint * __DS402Node_%(location)s.axis->RatioNumerator / __DS402Node_%(location)s.axis->RatioDenominator); |
128 %(publish_variables)s |
|
129 } |
135 } |