|
1 #!/usr/bin/env python |
|
2 # -*- coding: utf-8 -*- |
|
3 |
|
4 # This file is part of Beremiz |
|
5 # |
|
6 # Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT |
|
7 # RTES Lab : CRKim, JBLee, youcu |
|
8 # Higen Motor : Donggu Kang |
|
9 # |
|
10 # See COPYING file for copyrights details. |
|
11 |
|
12 import os |
|
13 |
|
14 import wx |
|
15 |
|
16 from PLCControler import LOCATION_CONFNODE, LOCATION_MODULE, LOCATION_GROUP, LOCATION_VAR_INPUT, LOCATION_VAR_OUTPUT, LOCATION_VAR_MEMORY |
|
17 |
|
18 from MotionLibrary import Headers, AxisXSD |
|
19 from EthercatSlave import _EthercatSlaveCTN, _CommonSlave |
|
20 from ConfigEditor import CIA402NodeEditor |
|
21 |
|
22 # Definition of node variables that have to be mapped in PDO |
|
23 # [(name, index, subindex, type, |
|
24 # direction for master ('I': input, 'Q': output)),...] |
|
25 NODE_VARIABLES = [ |
|
26 ("ControlWord", 0x6040, 0x00, "UINT", "Q"), |
|
27 ("TargetPosition", 0x607a, 0x00, "DINT", "Q"), |
|
28 ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"), |
|
29 ("TargetTorque", 0x6071, 0x00, "INT", "Q"), |
|
30 ("ModesOfOperation", 0x6060, 0x00, "SINT", "Q"), |
|
31 ("StatusWord", 0x6041, 0x00, "UINT", "I"), |
|
32 ("ModesOfOperationDisplay", 0x6061, 0x00, "SINT", "I"), |
|
33 ("ActualPosition", 0x6064, 0x00, "DINT", "I"), |
|
34 ("ActualVelocity", 0x606c, 0x00, "DINT", "I"), |
|
35 ("ActualTorque", 0x6077, 0x00, "INT", "I"), |
|
36 ] |
|
37 |
|
38 # Definition of optional node variables that can be added to PDO mapping. |
|
39 # A checkbox will be displayed for each section in node configuration panel to |
|
40 # enable them |
|
41 # [(section_name, |
|
42 # [{'description', (name, index, subindex, type, |
|
43 # direction for master ('I': input, 'Q': output)), |
|
44 # 'retrieve', string_template_for_retrieve_variable (None: not retrieved, |
|
45 # default string template if not defined), |
|
46 # 'publish', string_template_for_publish_variable (None: not published, |
|
47 # default string template if not defined), |
|
48 # },...] |
|
49 EXTRA_NODE_VARIABLES = [ |
|
50 ("ErrorCode", [ |
|
51 {"description": ("ErrorCode", 0x603F, 0x00, "UINT", "I"), |
|
52 "publish": None} |
|
53 ]), |
|
54 ("DigitalInputs", [ |
|
55 {"description": ("DigitalInputs", 0x60FD, 0x00, "UDINT", "I"), |
|
56 "publish": None} |
|
57 ]), |
|
58 ("DigitalOutputs", [ |
|
59 {"description": ("DigitalOutputs", 0x60FE, 0x00, "UDINT", "Q"), |
|
60 "retrieve": None} |
|
61 ]), |
|
62 ("TouchProbe", [ |
|
63 {"description": ("TouchProbeFunction", 0x60B8, 0x00, "UINT", "Q"), |
|
64 "retrieve": None}, |
|
65 {"description": ("TouchProbeStatus", 0x60B9, 0x00, "UINT", "I"), |
|
66 "publish": None}, |
|
67 {"description": ("TouchProbePos1PosValue", 0x60BA, 0x00, "DINT", "I"), |
|
68 "publish": None}, |
|
69 {"description": ("TouchProbePos1NegValue", 0x60BB, 0x00, "DINT", "I"), |
|
70 "publish": None}, |
|
71 ]), |
|
72 ] |
|
73 |
|
74 # List of parameters name in no configuration panel for optional variable |
|
75 # sections |
|
76 EXTRA_NODE_VARIABLES_DICT = { |
|
77 "Enable" + name: params |
|
78 for name, params in EXTRA_NODE_VARIABLES} |
|
79 |
|
80 # List of block to define to interface MCL to fieldbus for specific functions |
|
81 FIELDBUS_INTERFACE_GLOBAL_INSTANCES = [ |
|
82 {"blocktype": "GetTorqueLimit", |
|
83 "inputs": [], |
|
84 "outputs": [{"name": "TorqueLimitPos", "type": "UINT"}, |
|
85 {"name": "TorqueLimitNeg", "type": "UINT"}]}, |
|
86 {"blocktype": "SetTorqueLimit", |
|
87 "inputs": [{"name": "TorqueLimitPos", "type": "UINT"}, |
|
88 {"name": "TorqueLimitNeg", "type": "UINT"}], |
|
89 "outputs": []}, |
|
90 ] |
|
91 |
|
92 #-------------------------------------------------- |
|
93 # Ethercat CIA402 Node |
|
94 #-------------------------------------------------- |
|
95 |
|
96 class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN): |
|
97 XSD = """<?xml version="1.0" encoding="ISO-8859-1" ?> |
|
98 <xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema"> |
|
99 <xsd:element name="CIA402SlaveParams"> |
|
100 <xsd:complexType> |
|
101 %s |
|
102 </xsd:complexType> |
|
103 </xsd:element> |
|
104 </xsd:schema> |
|
105 """ % ("\n".join(["""\ |
|
106 <xsd:attribute name="Enable%s" type="xsd:boolean" |
|
107 use="optional" default="false"/>""" % category |
|
108 for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD) |
|
109 |
|
110 NODE_PROFILE = 402 |
|
111 EditorType = CIA402NodeEditor |
|
112 |
|
113 ConfNodeMethods = [ |
|
114 {"bitmap" : "CIA402AxisRef", |
|
115 "name" : _("Axis Ref"), |
|
116 "tooltip" : _("Initiate Drag'n drop of Axis ref located variable"), |
|
117 "method" : "_getCIA402AxisRef", |
|
118 "push": True}, |
|
119 {"bitmap" : "CIA402NetPos", |
|
120 "name" : _("Axis Pos"), |
|
121 "tooltip" : _("Initiate Drag'n drop of Network position located variable"), |
|
122 "method" : "_getCIA402NetworkPosition", |
|
123 "push": True}, |
|
124 ] |
|
125 |
|
126 #-------------------------------------------------- |
|
127 # class code |
|
128 #-------------------------------------------------- |
|
129 |
|
130 def __init__(self): |
|
131 # ----------- call ethercat mng. function -------------- |
|
132 self.CommonMethod = _CommonSlave(self) |
|
133 |
|
134 def GetIconName(self): |
|
135 return "CIA402Slave" |
|
136 |
|
137 def SetParamsAttribute(self, path, value): |
|
138 if path == "CIA402SlaveParams.Type": |
|
139 path = "SlaveParams.Type" |
|
140 elif path == "CIA402SlaveParams.Alias": |
|
141 path = "SlaveParams.Alias" |
|
142 return _EthercatSlaveCTN.SetParamsAttribute(self, path, value) |
|
143 |
|
144 def GetVariableLocationTree(self): |
|
145 axis_name = self.CTNName() |
|
146 current_location = self.GetCurrentLocation() |
|
147 children = [{"name": name_frmt % (axis_name), |
|
148 "type": LOCATION_VAR_INPUT, |
|
149 "size": "W", |
|
150 "IEC_type": iec_type, |
|
151 "var_name": var_name_frmt % axis_name, |
|
152 "location": location_frmt % ( |
|
153 ".".join(map(str, current_location))), |
|
154 "description": "", |
|
155 "children": []} |
|
156 for name_frmt, iec_type, var_name_frmt, location_frmt in |
|
157 [("%s Network Position", "UINT", "%s_pos", "%%IW%s"), |
|
158 ("%s Axis Ref", "AXIS_REF", "%s", "%%IW%s.402")]] |
|
159 children.extend(self.CTNParent.GetDeviceLocationTree( |
|
160 self.GetSlavePos(), current_location, axis_name)) |
|
161 return {"name": axis_name, |
|
162 "type": LOCATION_CONFNODE, |
|
163 "location": self.GetFullIEC_Channel(), |
|
164 "children": children, |
|
165 } |
|
166 |
|
167 def CTNGlobalInstances(self): |
|
168 current_location = self.GetCurrentLocation() |
|
169 return [("%s_%s" % (block_infos["blocktype"], |
|
170 "_".join(map(str, current_location))), |
|
171 "EtherLab%s" % block_infos["blocktype"], "") |
|
172 for block_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES] |
|
173 |
|
174 def StartDragNDrop(self, data): |
|
175 data_obj = wx.TextDataObject(str(data)) |
|
176 dragSource = wx.DropSource(self.GetCTRoot().AppFrame) |
|
177 dragSource.SetData(data_obj) |
|
178 dragSource.DoDragDrop() |
|
179 |
|
180 def _getCIA402NetworkPosition(self): |
|
181 self.StartDragNDrop( |
|
182 ("%%IW%s" % ".".join(map(str, self.GetCurrentLocation())), |
|
183 "location", "UINT", self.CTNName() + "_Pos", "")) |
|
184 |
|
185 def _getCIA402AxisRef(self): |
|
186 self.StartDragNDrop( |
|
187 ("%%IW%s.402" % ".".join(map(str, self.GetCurrentLocation())), |
|
188 "location", "AXIS_REF", self.CTNName(), "")) |
|
189 |
|
190 def CTNGenerate_C(self, buildpath, locations): |
|
191 current_location = self.GetCurrentLocation() |
|
192 |
|
193 location_str = "_".join(map(lambda x:str(x), current_location)) |
|
194 slave_pos = self.GetSlavePos() |
|
195 MCL_headers = Headers |
|
196 |
|
197 # Open CIA402 node code template file |
|
198 plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], |
|
199 "plc_cia402node.c") |
|
200 plc_cia402node_file = open(plc_cia402node_filepath, 'r') |
|
201 plc_cia402node_code = plc_cia402node_file.read() |
|
202 plc_cia402node_file.close() |
|
203 |
|
204 # Init list of generated strings for each code template file section |
|
205 fieldbus_interface_declaration = [] |
|
206 fieldbus_interface_definition = [] |
|
207 init_axis_params = [] |
|
208 extra_variables_retrieve = [] |
|
209 extra_variables_publish = [] |
|
210 extern_located_variables_declaration = [] |
|
211 entry_variables = [] |
|
212 init_entry_variables = [] |
|
213 |
|
214 # Fieldbus interface code sections |
|
215 for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES: |
|
216 blocktype = blocktype_infos["blocktype"] |
|
217 ucase_blocktype = blocktype.upper() |
|
218 blockname = "_".join([ucase_blocktype, location_str]) |
|
219 |
|
220 extract_inputs = "\n".join(["""\ |
|
221 __SET_VAR(%s->, %s,, %s);""" % (blockname, input_name, input_value) |
|
222 for (input_name, input_value) in [ |
|
223 ("EXECUTE", "__GET_VAR(data__->EXECUTE)")] + [ |
|
224 (input["name"].upper(), |
|
225 "__GET_VAR(data__->%s)" % input["name"].upper()) |
|
226 for input in blocktype_infos["inputs"]] |
|
227 ]) |
|
228 |
|
229 |
|
230 return_outputs = "\n".join(["""\ |
|
231 __SET_VAR(data__->,%(output_name)s,, |
|
232 __GET_VAR(%(blockname)s->%(output_name)s));""" % locals() |
|
233 for output_name in ["DONE", "BUSY", "ERROR"] + [ |
|
234 output["name"].upper() |
|
235 for output in blocktype_infos["outputs"]] |
|
236 ]) |
|
237 |
|
238 fieldbus_interface_declaration.append(""" |
|
239 extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__); |
|
240 void __%(blocktype)s_%(location_str)s(MC_%(ucase_blocktype)s *data__) { |
|
241 __DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s); |
|
242 ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s(); |
|
243 __SET_VAR(%(blockname)s->, POS,, AxsPub.axis->NetworkPosition); |
|
244 %(extract_inputs)s |
|
245 ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s); |
|
246 %(return_outputs)s |
|
247 }""" % locals()) |
|
248 |
|
249 fieldbus_interface_definition.append("""\ |
|
250 AxsPub.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location_str)s;\ |
|
251 """ % locals()) |
|
252 |
|
253 # Get a copy list of default variables to map |
|
254 variables = NODE_VARIABLES[:] |
|
255 |
|
256 # Set AxisRef public struct members value |
|
257 node_params = self.CTNParams[1].getElementInfos(self.CTNParams[0]) |
|
258 for param in node_params["children"]: |
|
259 param_name = param["name"] |
|
260 |
|
261 # Param is optional variables section enable flag |
|
262 extra_node_variable_infos = EXTRA_NODE_VARIABLES_DICT.get(param_name) |
|
263 if extra_node_variable_infos is not None: |
|
264 param_name = param_name.replace("Enable", "") + "Enabled" |
|
265 |
|
266 if not param["value"]: |
|
267 continue |
|
268 |
|
269 # Optional variables section is enabled |
|
270 for variable_infos in extra_node_variable_infos: |
|
271 var_name = variable_infos["description"][0] |
|
272 |
|
273 # Add each variables defined in section description to the |
|
274 # list of variables to map |
|
275 variables.append(variable_infos["description"]) |
|
276 |
|
277 # Add code to publish or retrive variable |
|
278 for var_exchange_dir, str_list, default_template in [ |
|
279 ("retrieve", extra_variables_retrieve, |
|
280 " AxsPub.axis->%(var_name)s = *(AxsPub.%(var_name)s);"), |
|
281 ("publish", extra_variables_publish, |
|
282 " *(AxsPub.%(var_name)s) = AxsPub.axis->%(var_name)s;")]: |
|
283 |
|
284 template = variable_infos.get(var_exchange_dir, |
|
285 default_template) |
|
286 if template is not None: |
|
287 extra_variables_publish.append(template % locals()) |
|
288 |
|
289 # Set AxisRef public struct member value if defined |
|
290 if param["value"] is not None: |
|
291 param_value = ({True: "1", False: "0"}[param["value"]] |
|
292 if param["type"] == "boolean" |
|
293 else str(param["value"])) |
|
294 |
|
295 init_axis_params.append("""\ |
|
296 AxsPub.axis->%(param_name)s = %(param_value)s;""" % locals()) |
|
297 |
|
298 # Add each variable in list of variables to map to master list of |
|
299 # variables to add to network configuration |
|
300 for name, index, subindex, var_type, dir in variables: |
|
301 var_size = self.GetSizeOfType(var_type) |
|
302 var_name = """\ |
|
303 __%(dir)s%(var_size)s%(location_str)s_%(index)d_%(subindex)d""" % locals() |
|
304 |
|
305 extern_located_variables_declaration.append( |
|
306 "IEC_%(var_type)s *%(var_name)s;" % locals()) |
|
307 entry_variables.append( |
|
308 " IEC_%(var_type)s *%(name)s;" % locals()) |
|
309 init_entry_variables.append( |
|
310 " AxsPub.%(name)s = %(var_name)s;" % locals()) |
|
311 |
|
312 self.CTNParent.FileGenerator.DeclareVariable( |
|
313 slave_pos, index, subindex, var_type, dir, var_name) |
|
314 |
|
315 # Add newline between string in list of generated strings for sections |
|
316 [fieldbus_interface_declaration, fieldbus_interface_definition, |
|
317 init_axis_params, extra_variables_retrieve, extra_variables_publish, |
|
318 extern_located_variables_declaration, entry_variables, |
|
319 init_entry_variables] = map(lambda l: "\n".join(l), [ |
|
320 fieldbus_interface_declaration, fieldbus_interface_definition, |
|
321 init_axis_params, extra_variables_retrieve, extra_variables_publish, |
|
322 extern_located_variables_declaration, entry_variables, |
|
323 init_entry_variables]) |
|
324 |
|
325 # Write generated content to CIA402 node file |
|
326 Gen_CIA402Nodefile_path = os.path.join(buildpath, |
|
327 "cia402node_%s.c"%location_str) |
|
328 cia402nodefile = open(Gen_CIA402Nodefile_path, 'w') |
|
329 cia402nodefile.write(plc_cia402node_code % locals()) |
|
330 cia402nodefile.close() |
|
331 |
|
332 return [(Gen_CIA402Nodefile_path, '"-I%s"'%os.path.abspath(self.GetCTRoot().GetIECLibPath()))],"",True |