edouard@2165: #!/usr/bin/env python edouard@2165: # -*- coding: utf-8 -*- edouard@2165: edouard@2165: # This file is part of Beremiz edouard@2165: # edouard@2165: # Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT edouard@2165: # RTES Lab : CRKim, JBLee, youcu edouard@2165: # Higen Motor : Donggu Kang edouard@2165: # edouard@2165: # See COPYING file for copyrights details. edouard@2165: andrej@2405: from __future__ import absolute_import Laurent@2111: import os Laurent@2111: Laurent@2111: import wx Laurent@2111: andrej@2396: from PLCControler import LOCATION_CONFNODE, LOCATION_VAR_INPUT Laurent@2111: andrej@2406: from MotionLibrary import AxisXSD andrej@2405: from etherlab.EthercatSlave import _EthercatSlaveCTN, _CommonSlave andrej@2405: from etherlab.ConfigEditor import CIA402NodeEditor Laurent@2111: Laurent@2153: # Definition of node variables that have to be mapped in PDO andrej@2355: # [(name, index, subindex, type, Laurent@2153: # direction for master ('I': input, 'Q': output)),...] Laurent@2111: NODE_VARIABLES = [ Laurent@2153: ("ControlWord", 0x6040, 0x00, "UINT", "Q"), Laurent@2153: ("TargetPosition", 0x607a, 0x00, "DINT", "Q"), Laurent@2153: ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"), Laurent@2153: ("TargetTorque", 0x6071, 0x00, "INT", "Q"), Laurent@2153: ("ModesOfOperation", 0x6060, 0x00, "SINT", "Q"), Laurent@2153: ("StatusWord", 0x6041, 0x00, "UINT", "I"), Laurent@2153: ("ModesOfOperationDisplay", 0x6061, 0x00, "SINT", "I"), Laurent@2153: ("ActualPosition", 0x6064, 0x00, "DINT", "I"), Laurent@2153: ("ActualVelocity", 0x606c, 0x00, "DINT", "I"), Laurent@2153: ("ActualTorque", 0x6077, 0x00, "INT", "I"), Laurent@2111: ] Laurent@2111: Laurent@2153: # Definition of optional node variables that can be added to PDO mapping. Laurent@2153: # A checkbox will be displayed for each section in node configuration panel to Laurent@2153: # enable them andrej@2355: # [(section_name, andrej@2355: # [{'description', (name, index, subindex, type, Laurent@2153: # direction for master ('I': input, 'Q': output)), andrej@2355: # 'retrieve', string_template_for_retrieve_variable (None: not retrieved, Laurent@2153: # default string template if not defined), andrej@2355: # 'publish', string_template_for_publish_variable (None: not published, Laurent@2153: # default string template if not defined), Laurent@2153: # },...] Laurent@2111: EXTRA_NODE_VARIABLES = [ Laurent@2111: ("ErrorCode", [ Laurent@2111: {"description": ("ErrorCode", 0x603F, 0x00, "UINT", "I"), Laurent@2111: "publish": None} Laurent@2111: ]), Laurent@2111: ("DigitalInputs", [ Laurent@2111: {"description": ("DigitalInputs", 0x60FD, 0x00, "UDINT", "I"), Laurent@2111: "publish": None} Laurent@2111: ]), Laurent@2111: ("DigitalOutputs", [ Laurent@2111: {"description": ("DigitalOutputs", 0x60FE, 0x00, "UDINT", "Q"), Laurent@2111: "retrieve": None} Laurent@2146: ]), Laurent@2146: ("TouchProbe", [ Laurent@2146: {"description": ("TouchProbeFunction", 0x60B8, 0x00, "UINT", "Q"), Laurent@2146: "retrieve": None}, Laurent@2146: {"description": ("TouchProbeStatus", 0x60B9, 0x00, "UINT", "I"), Laurent@2146: "publish": None}, Laurent@2146: {"description": ("TouchProbePos1PosValue", 0x60BA, 0x00, "DINT", "I"), Laurent@2146: "publish": None}, Laurent@2146: {"description": ("TouchProbePos1NegValue", 0x60BB, 0x00, "DINT", "I"), Laurent@2146: "publish": None}, Laurent@2146: ]), Laurent@2111: ] Laurent@2153: Laurent@2153: # List of parameters name in no configuration panel for optional variable Laurent@2153: # sections Laurent@2153: EXTRA_NODE_VARIABLES_DICT = { andrej@2355: "Enable" + name: params Laurent@2153: for name, params in EXTRA_NODE_VARIABLES} Laurent@2153: Laurent@2153: # List of block to define to interface MCL to fieldbus for specific functions Laurent@2153: FIELDBUS_INTERFACE_GLOBAL_INSTANCES = [ andrej@2355: {"blocktype": "GetTorqueLimit", Laurent@2111: "inputs": [], Laurent@2111: "outputs": [{"name": "TorqueLimitPos", "type": "UINT"}, Laurent@2111: {"name": "TorqueLimitNeg", "type": "UINT"}]}, andrej@2355: {"blocktype": "SetTorqueLimit", Laurent@2111: "inputs": [{"name": "TorqueLimitPos", "type": "UINT"}, Laurent@2111: {"name": "TorqueLimitNeg", "type": "UINT"}], Laurent@2111: "outputs": []}, Laurent@2111: ] Laurent@2111: andrej@2356: # -------------------------------------------------- Laurent@2111: # Ethercat CIA402 Node andrej@2356: # -------------------------------------------------- Laurent@2111: andrej@2360: Laurent@2111: class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN): Laurent@2111: XSD = """ Laurent@2111: Laurent@2111: Laurent@2111: Laurent@2111: %s Laurent@2111: Laurent@2111: Laurent@2111: Laurent@2153: """ % ("\n".join(["""\ Laurent@2153: """ % category andrej@2381: for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD) andrej@2355: Laurent@2111: NODE_PROFILE = 402 Laurent@2111: EditorType = CIA402NodeEditor andrej@2355: Laurent@2111: ConfNodeMethods = [ andrej@2375: { andrej@2375: "bitmap": "CIA402AxisRef", andrej@2375: "name": _("Axis Ref"), andrej@2375: "tooltip": _("Initiate Drag'n drop of Axis ref located variable"), andrej@2375: "method": "_getCIA402AxisRef", andrej@2375: "push": True, andrej@2375: }, andrej@2375: { andrej@2375: "bitmap": "CIA402NetPos", andrej@2375: "name": _("Axis Pos"), andrej@2375: "tooltip": _("Initiate Drag'n drop of Network position located variable"), andrej@2375: "method": "_getCIA402NetworkPosition", andrej@2375: "push": True, andrej@2375: }, Laurent@2153: ] andrej@2355: andrej@2356: # -------------------------------------------------- Edouard@2152: # class code andrej@2356: # -------------------------------------------------- andrej@2355: Edouard@2152: def __init__(self): andrej@2393: _EthercatSlaveCTN.__init__(self) andrej@2393: Edouard@2152: # ----------- call ethercat mng. function -------------- Edouard@2152: self.CommonMethod = _CommonSlave(self) andrej@2355: Laurent@2111: def GetIconName(self): Laurent@2111: return "CIA402Slave" andrej@2355: Laurent@2111: def SetParamsAttribute(self, path, value): Laurent@2111: if path == "CIA402SlaveParams.Type": Laurent@2111: path = "SlaveParams.Type" Laurent@2111: elif path == "CIA402SlaveParams.Alias": Laurent@2111: path = "SlaveParams.Alias" Laurent@2111: return _EthercatSlaveCTN.SetParamsAttribute(self, path, value) andrej@2355: Laurent@2111: def GetVariableLocationTree(self): Laurent@2111: axis_name = self.CTNName() Laurent@2111: current_location = self.GetCurrentLocation() andrej@2380: children = [ andrej@2380: { andrej@2380: "name": name_frmt % (axis_name), andrej@2380: "type": LOCATION_VAR_INPUT, andrej@2380: "size": "W", andrej@2380: "IEC_type": iec_type, andrej@2380: "var_name": var_name_frmt % axis_name, andrej@2380: "location": location_frmt % (".".join(map(str, current_location))), andrej@2380: "description": "", andrej@2380: "children": [] andrej@2380: } andrej@2380: for name_frmt, iec_type, var_name_frmt, location_frmt in [ andrej@2407: ("%s Network Position", "UINT", "%s_pos", "%%IW%s"), andrej@2407: ("%s Axis Ref", "AXIS_REF", "%s", "%%IW%s.402") andrej@2380: ] andrej@2380: ] andrej@2407: children.extend(self.CTNParent.GetDeviceLocationTree(self.GetSlavePos(), andrej@2407: current_location, andrej@2407: axis_name)) andrej@2362: return { andrej@2362: "name": axis_name, andrej@2362: "type": LOCATION_CONFNODE, andrej@2362: "location": self.GetFullIEC_Channel(), andrej@2362: "children": children, Laurent@2111: } andrej@2355: Laurent@2111: def CTNGlobalInstances(self): Laurent@2111: current_location = self.GetCurrentLocation() andrej@2355: return [("%s_%s" % (block_infos["blocktype"], Laurent@2153: "_".join(map(str, current_location))), andrej@2355: "EtherLab%s" % block_infos["blocktype"], "") Laurent@2153: for block_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES] andrej@2355: Laurent@2154: def StartDragNDrop(self, data): Laurent@2154: data_obj = wx.TextDataObject(str(data)) Laurent@2154: dragSource = wx.DropSource(self.GetCTRoot().AppFrame) Laurent@2154: dragSource.SetData(data_obj) Laurent@2154: dragSource.DoDragDrop() andrej@2355: Laurent@2154: def _getCIA402NetworkPosition(self): Laurent@2154: self.StartDragNDrop( andrej@2355: ("%%IW%s" % ".".join(map(str, self.GetCurrentLocation())), Laurent@2154: "location", "UINT", self.CTNName() + "_Pos", "")) andrej@2355: Laurent@2111: def _getCIA402AxisRef(self): Laurent@2154: self.StartDragNDrop( andrej@2355: ("%%IW%s.402" % ".".join(map(str, self.GetCurrentLocation())), Laurent@2154: "location", "AXIS_REF", self.CTNName(), "")) andrej@2355: Laurent@2111: def CTNGenerate_C(self, buildpath, locations): Laurent@2111: current_location = self.GetCurrentLocation() andrej@2355: andrej@2391: location_str = "_".join(map(str, current_location)) Laurent@2153: slave_pos = self.GetSlavePos() andrej@2355: andrej@2355: # Open CIA402 node code template file andrej@2355: plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], Laurent@2153: "plc_cia402node.c") Laurent@2111: plc_cia402node_file = open(plc_cia402node_filepath, 'r') Laurent@2111: plc_cia402node_code = plc_cia402node_file.read() Laurent@2111: plc_cia402node_file.close() andrej@2355: Laurent@2153: # Init list of generated strings for each code template file section Laurent@2153: fieldbus_interface_declaration = [] Laurent@2153: fieldbus_interface_definition = [] Laurent@2153: init_axis_params = [] Laurent@2153: extra_variables_retrieve = [] Laurent@2153: extra_variables_publish = [] Laurent@2153: extern_located_variables_declaration = [] Laurent@2153: entry_variables = [] Laurent@2153: init_entry_variables = [] andrej@2355: Laurent@2153: # Fieldbus interface code sections Laurent@2153: for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES: Laurent@2153: blocktype = blocktype_infos["blocktype"] Laurent@2153: ucase_blocktype = blocktype.upper() Laurent@2153: blockname = "_".join([ucase_blocktype, location_str]) andrej@2355: andrej@2407: extract_inputs = "\n".join([ andrej@2407: """\ andrej@2407: __SET_VAR(%s->, %s,, %s);""" % (blockname, input_name, input_value) andrej@2407: for (input_name, input_value) in andrej@2407: [("EXECUTE", "__GET_VAR(data__->EXECUTE)")] + [ andrej@2355: (input["name"].upper(), Laurent@2153: "__GET_VAR(data__->%s)" % input["name"].upper()) andrej@2407: for input in blocktype_infos["inputs"] andrej@2407: ]]) andrej@2355: andrej@2381: return_outputs = "\n".join([ andrej@2381: """\ andrej@2406: __SET_VAR(data__->,%(output_name)s,, andrej@2406: __GET_VAR(%(blockname)s->%(output_name)s));""" % { andrej@2406: "output_name": output_name, andrej@2406: "blockname": blockname andrej@2406: } andrej@2381: for output_name in ["DONE", "BUSY", "ERROR"] + [ andrej@2407: output["name"].upper() andrej@2407: for output in blocktype_infos["outputs"]] andrej@2381: ]) andrej@2355: andrej@2406: loc_dict = { andrej@2406: "ucase_blocktype": ucase_blocktype, andrej@2406: "blocktype": blocktype, andrej@2406: "blockname": blockname, andrej@2406: "location_str": location_str, andrej@2406: "extract_inputs": extract_inputs, andrej@2406: "return_outputs": return_outputs, andrej@2406: } andrej@2406: Laurent@2153: fieldbus_interface_declaration.append(""" Laurent@2153: extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__); Laurent@2153: void __%(blocktype)s_%(location_str)s(MC_%(ucase_blocktype)s *data__) { Laurent@2153: __DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s); Laurent@2153: ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s(); Edouard@2164: __SET_VAR(%(blockname)s->, POS,, AxsPub.axis->NetworkPosition); Laurent@2153: %(extract_inputs)s Laurent@2153: ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s); Laurent@2153: %(return_outputs)s andrej@2406: }""" % loc_dict) andrej@2355: Laurent@2153: fieldbus_interface_definition.append("""\ Laurent@2153: AxsPub.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location_str)s;\ andrej@2406: """ % loc_dict) andrej@2355: Laurent@2153: # Get a copy list of default variables to map Laurent@2111: variables = NODE_VARIABLES[:] andrej@2355: Laurent@2153: # Set AxisRef public struct members value Laurent@2153: node_params = self.CTNParams[1].getElementInfos(self.CTNParams[0]) Laurent@2153: for param in node_params["children"]: Laurent@2153: param_name = param["name"] andrej@2355: Laurent@2153: # Param is optional variables section enable flag Laurent@2153: extra_node_variable_infos = EXTRA_NODE_VARIABLES_DICT.get(param_name) Laurent@2153: if extra_node_variable_infos is not None: Laurent@2156: param_name = param_name.replace("Enable", "") + "Enabled" andrej@2355: Laurent@2153: if not param["value"]: Laurent@2153: continue andrej@2355: Laurent@2153: # Optional variables section is enabled Laurent@2153: for variable_infos in extra_node_variable_infos: Laurent@2153: var_name = variable_infos["description"][0] andrej@2355: Laurent@2153: # Add each variables defined in section description to the Laurent@2153: # list of variables to map Laurent@2153: variables.append(variable_infos["description"]) andrej@2355: Laurent@2153: # Add code to publish or retrive variable andrej@2406: coded = [ andrej@2406: ("retrieve", andrej@2406: extra_variables_retrieve, andrej@2406: " AxsPub.axis->%(var_name)s = *(AxsPub.%(var_name)s);"), andrej@2406: ("publish", andrej@2406: extra_variables_publish, andrej@2406: " *(AxsPub.%(var_name)s) = AxsPub.axis->%(var_name)s;") andrej@2406: ] andrej@2406: for var_exchange_dir, _str_list, default_template in coded: andrej@2406: template = variable_infos.get(var_exchange_dir, default_template) Laurent@2153: if template is not None: Laurent@2153: extra_variables_publish.append(template % locals()) andrej@2355: Laurent@2153: # Set AxisRef public struct member value if defined Laurent@2155: if param["value"] is not None: Laurent@2153: param_value = ({True: "1", False: "0"}[param["value"]] Laurent@2153: if param["type"] == "boolean" Laurent@2153: else str(param["value"])) andrej@2355: Laurent@2153: init_axis_params.append("""\ andrej@2413: AxsPub.axis->%(param_name)s = %(param_value)s;""" % { andrej@2413: "param_value": param_value, andrej@2413: "param_name": param_name, andrej@2413: }) andrej@2355: Laurent@2153: # Add each variable in list of variables to map to master list of Laurent@2153: # variables to add to network configuration Laurent@2153: for name, index, subindex, var_type, dir in variables: Laurent@2153: var_size = self.GetSizeOfType(var_type) andrej@2406: loc_dict = { andrej@2406: "var_size": var_size, andrej@2406: "var_type": var_type, andrej@2406: "name:": name, andrej@2406: "location_str": location_str, andrej@2406: "index": index, andrej@2406: "subindex": subindex, andrej@2406: } Laurent@2153: var_name = """\ andrej@2406: __%(dir)s%(var_size)s%(location_str)s_%(index)d_%(subindex)d""" % loc_dict andrej@2406: loc_dict["var_name"] = var_name andrej@2355: Laurent@2153: extern_located_variables_declaration.append( andrej@2407: "IEC_%(var_type)s *%(var_name)s;" % loc_dict) Laurent@2153: entry_variables.append( andrej@2407: " IEC_%(var_type)s *%(name)s;" % loc_dict) Laurent@2153: init_entry_variables.append( andrej@2407: " AxsPub.%(name)s = %(var_name)s;" % loc_dict) andrej@2355: Laurent@2111: self.CTNParent.FileGenerator.DeclareVariable( andrej@2407: slave_pos, index, subindex, var_type, dir, var_name) andrej@2355: Laurent@2153: # Add newline between string in list of generated strings for sections Laurent@2153: [fieldbus_interface_declaration, fieldbus_interface_definition, Laurent@2153: init_axis_params, extra_variables_retrieve, extra_variables_publish, andrej@2355: extern_located_variables_declaration, entry_variables, andrej@2391: init_entry_variables] = map("\n".join, [ andrej@2407: fieldbus_interface_declaration, fieldbus_interface_definition, andrej@2407: init_axis_params, extra_variables_retrieve, extra_variables_publish, andrej@2407: extern_located_variables_declaration, entry_variables, andrej@2407: init_entry_variables]) andrej@2355: Laurent@2153: # Write generated content to CIA402 node file andrej@2355: Gen_CIA402Nodefile_path = os.path.join(buildpath, andrej@2381: "cia402node_%s.c" % location_str) Laurent@2111: cia402nodefile = open(Gen_CIA402Nodefile_path, 'w') Laurent@2153: cia402nodefile.write(plc_cia402node_code % locals()) Laurent@2111: cia402nodefile.close() andrej@2355: andrej@2363: return [(Gen_CIA402Nodefile_path, '"-I%s"' % os.path.abspath(self.GetCTRoot().GetIECLibPath()))], "", True