Laurent@2111: import os
Laurent@2111: 
Laurent@2111: import wx
Laurent@2111: 
Laurent@2111: from PLCControler import LOCATION_CONFNODE, LOCATION_MODULE, LOCATION_GROUP, LOCATION_VAR_INPUT, LOCATION_VAR_OUTPUT, LOCATION_VAR_MEMORY
Laurent@2111: 
Laurent@2111: from MotionLibrary import Headers, AxisXSD
Laurent@2111: from EthercatSlave import _EthercatSlaveCTN
Laurent@2111: from ConfigEditor import CIA402NodeEditor
Laurent@2111: 
Laurent@2111: NODE_VARIABLES = [
Laurent@2111:     ("ControlWord", 0x6040, 0x00, "UINT", "Q"),
Laurent@2111:     ("TargetPosition", 0x607a, 0x00, "DINT", "Q"),
Laurent@2136:     ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"),
Laurent@2136:     ("TargetTorque", 0x6071, 0x00, "INT", "Q"),
Laurent@2111:     ("ModesOfOperation", 0x06060, 0x00, "SINT", "Q"),
Laurent@2111:     ("StatusWord", 0x6041, 0x00, "UINT", "I"),
Laurent@2111:     ("ModesOfOperationDisplay", 0x06061, 0x00, "SINT", "I"),
Laurent@2111:     ("ActualPosition", 0x6064, 0x00, "DINT", "I"),
Laurent@2136:     ("ActualVelocity", 0x606c, 0x00, "DINT", "I"),
Laurent@2136:     ("ActualTorque", 0x6077, 0x00, "INT", "I"),
Laurent@2111: ]
Laurent@2111: 
Laurent@2111: DEFAULT_RETRIEVE = "    __CIA402Node_%(location)s.axis->%(name)s = *(__CIA402Node_%(location)s.%(name)s);"
Laurent@2111: DEFAULT_PUBLISH = "    *(__CIA402Node_%(location)s.%(name)s) = __CIA402Node_%(location)s.axis->%(name)s;"
Laurent@2111: 
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@2111:         ])
Laurent@2111: ]
Laurent@2111: EXTRA_NODE_VARIABLES_DICT = dict([("Enable" + name, value) for name, value in EXTRA_NODE_VARIABLES])
Laurent@2111: 
Laurent@2111: BLOCK_INPUT_TEMPLATE = "    __SET_VAR(%(blockname)s->,%(input_name)s, %(input_value)s);"
Laurent@2111: BLOCK_OUTPUT_TEMPLATE = "    __SET_VAR(data__->,%(output_name)s, __GET_VAR(%(blockname)s->%(output_name)s));"
Laurent@2111: 
Laurent@2111: BLOCK_FUNCTION_TEMPLATE = """
Laurent@2111: extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__);
Laurent@2111: void __%(blocktype)s_%(location)s(MC_%(ucase_blocktype)s *data__) {
Laurent@2111: __DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s);
Laurent@2111: ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s();
Laurent@2111: %(extract_inputs)s
Laurent@2111: ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s);
Laurent@2111: %(return_outputs)s
Laurent@2111: }
Laurent@2111: """
Laurent@2111: 
Laurent@2118: BLOCK_FUNTION_DEFINITION_TEMPLATE = "        __CIA402Node_%(location)s.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location)s;"
Laurent@2111: 
Laurent@2111: GLOBAL_INSTANCES = [
Laurent@2111:     {"blocktype": "GetTorqueLimit", 
Laurent@2111:      "inputs": [],
Laurent@2111:      "outputs": [{"name": "TorqueLimitPos", "type": "UINT"},
Laurent@2111:                  {"name": "TorqueLimitNeg", "type": "UINT"}]},
Laurent@2111:     {"blocktype": "SetTorqueLimit", 
Laurent@2111:      "inputs": [{"name": "TorqueLimitPos", "type": "UINT"},
Laurent@2111:                 {"name": "TorqueLimitNeg", "type": "UINT"}],
Laurent@2111:      "outputs": []},
Laurent@2111: ]
Laurent@2111: 
Laurent@2111: #--------------------------------------------------
Laurent@2111: #                 Ethercat CIA402 Node
Laurent@2111: #--------------------------------------------------
Laurent@2111: 
Laurent@2111: class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):
Laurent@2111:     XSD = """<?xml version="1.0" encoding="ISO-8859-1" ?>
Laurent@2111:     <xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">
Laurent@2111:       <xsd:element name="CIA402SlaveParams">
Laurent@2111:         <xsd:complexType>
Laurent@2111:           %s
Laurent@2111:         </xsd:complexType>
Laurent@2111:       </xsd:element>
Laurent@2111:     </xsd:schema>
Laurent@2111:     """ % ("\n".join(['<xsd:attribute name="Enable%s" type="xsd:boolean" use="optional" default="false"/>' % category 
Laurent@2111:                       for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)
Laurent@2111:     
Laurent@2111:     NODE_PROFILE = 402
Laurent@2111:     EditorType = CIA402NodeEditor
Laurent@2111:     
Laurent@2111:     ConfNodeMethods = [
Laurent@2111:         {"bitmap" : "CIA402AxisRef",
Laurent@2111:          "name" : _("Axis Ref"),
Laurent@2111:          "tooltip" : _("Initiate Drag'n drop of Axis ref located variable"),
Laurent@2111:          "method" : "_getCIA402AxisRef",
Laurent@2111:          "push": True},
Laurent@2111:     ]
Laurent@2111:     
Laurent@2111:     def GetIconName(self):
Laurent@2111:         return "CIA402Slave"
Laurent@2111:     
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)
Laurent@2111:     
Laurent@2111:     def GetVariableLocationTree(self):
Laurent@2111:         axis_name = self.CTNName()
Laurent@2111:         current_location = self.GetCurrentLocation()
Laurent@2111:         children = [{"name": "%s Axis Ref" % (axis_name),
Laurent@2111:                      "type": LOCATION_VAR_INPUT,
Laurent@2111:                      "size": "W",
Laurent@2111:                      "IEC_type": "AXIS_REF",
Laurent@2111:                      "var_name": axis_name,
Laurent@2111:                      "location": "%%IW%s.0" % (".".join(map(str, current_location))),
Laurent@2111:                      "description": "",
Laurent@2111:                      "children": []}]
Laurent@2111:         children.extend(self.CTNParent.GetDeviceLocationTree(self.GetSlavePos(), current_location, axis_name))
Laurent@2111:         return  {"name": axis_name,
Laurent@2111:                  "type": LOCATION_CONFNODE,
Laurent@2111:                  "location": self.GetFullIEC_Channel(),
Laurent@2111:                  "children": children,
Laurent@2111:         }
Laurent@2111:     
Laurent@2111:     def CTNGlobalInstances(self):
Laurent@2111:         current_location = self.GetCurrentLocation()
Laurent@2111:         return [("%s_%s" % (block_infos["blocktype"], "_".join(map(str, current_location))),
Laurent@2111:                  "EtherLab%s" % block_infos["blocktype"]) for block_infos in GLOBAL_INSTANCES]
Laurent@2111:     
Laurent@2111:     def _getCIA402AxisRef(self):
Laurent@2111:         data = wx.TextDataObject(str(("%%IW%s.0" % ".".join(map(str, self.GetCurrentLocation())), 
Laurent@2111:                                       "location", "AXIS_REF", self.CTNName(), "")))
Laurent@2111:         dragSource = wx.DropSource(self.GetCTRoot().AppFrame)
Laurent@2111:         dragSource.SetData(data)
Laurent@2111:         dragSource.DoDragDrop()
Laurent@2111:     
Laurent@2111:     def CTNGenerate_C(self, buildpath, locations):
Laurent@2111:         current_location = self.GetCurrentLocation()
Laurent@2111:         
Laurent@2111:         location_str = "_".join(map(lambda x:str(x), current_location))
Laurent@2111:         
Laurent@2111:         plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], "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()
Laurent@2111:         
Laurent@2111:         str_completion = {
Laurent@2111:             "slave_pos": self.GetSlavePos(),
Laurent@2111:             "location": location_str,
Laurent@2111:             "MCL_headers": Headers,
Laurent@2111:             "extern_located_variables_declaration": [],
Laurent@2111:             "fieldbus_interface_declaration": [],
Laurent@2111:             "fieldbus_interface_definition": [],
Laurent@2111:             "entry_variables": [],
Laurent@2111:             "init_axis_params": [],
Laurent@2111:             "init_entry_variables": [],
Laurent@2111:             "extra_variables_retrieve": [],
Laurent@2111:             "extra_variables_publish": []
Laurent@2111:         }
Laurent@2111:         
Laurent@2111:         for blocktype_infos in GLOBAL_INSTANCES:
Laurent@2111:             texts = {
Laurent@2111:                 "blocktype": blocktype_infos["blocktype"],
Laurent@2111:                 "ucase_blocktype": blocktype_infos["blocktype"].upper(),
Laurent@2111:                 "location": "_".join(map(str, current_location))
Laurent@2111:             }
Laurent@2111:             texts["blockname"] = "%(ucase_blocktype)s_%(location)s" % texts
Laurent@2111:             
Laurent@2111:             inputs = [{"input_name": "POS", "input_value": str(self.GetSlavePos())},
Laurent@2111:                       {"input_name": "EXECUTE", "input_value": "__GET_VAR(data__->EXECUTE)"}] +\
Laurent@2111:                      [{"input_name": input["name"].upper(), 
Laurent@2111:                        "input_value": "__GET_VAR(data__->%s)" % input["name"].upper()}
Laurent@2111:                       for input in blocktype_infos["inputs"]]
Laurent@2111:             input_texts = []
Laurent@2111:             for input_infos in inputs:
Laurent@2111:                 input_infos.update(texts)
Laurent@2111:                 input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos)
Laurent@2111:             texts["extract_inputs"] = "\n".join(input_texts)
Laurent@2111:             
Laurent@2111:             outputs = [{"output_name": output} for output in ["DONE", "BUSY", "ERROR"]] + \
Laurent@2111:                       [{"output_name": output["name"].upper()} for output in blocktype_infos["outputs"]]
Laurent@2111:             output_texts = []
Laurent@2111:             for output_infos in outputs:
Laurent@2111:                 output_infos.update(texts)
Laurent@2111:                 output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos)
Laurent@2111:             texts["return_outputs"] = "\n".join(output_texts)
Laurent@2111:             
Laurent@2111:             str_completion["fieldbus_interface_declaration"].append(
Laurent@2111:                     BLOCK_FUNCTION_TEMPLATE % texts)
Laurent@2111:             
Laurent@2111:             str_completion["fieldbus_interface_definition"].append(
Laurent@2111:                     BLOCK_FUNTION_DEFINITION_TEMPLATE % texts)
Laurent@2111:             
Laurent@2111:         variables = NODE_VARIABLES[:]
Laurent@2111:         
Laurent@2111:         params = self.CTNParams[1].getElementInfos(self.CTNParams[0])
Laurent@2111:         for param in params["children"]:
Laurent@2111:             if param["name"] in EXTRA_NODE_VARIABLES_DICT:
Laurent@2111:                 if param["value"]:
Laurent@2111:                     extra_variables = EXTRA_NODE_VARIABLES_DICT.get(param["name"])
Laurent@2111:                     for variable_infos in extra_variables:
Laurent@2111:                         var_infos = {
Laurent@2111:                             "location": location_str,
Laurent@2111:                             "name": variable_infos["description"][0]
Laurent@2111:                         }
Laurent@2111:                         variables.append(variable_infos["description"])
Laurent@2111:                         retrieve_template = variable_infos.get("retrieve", DEFAULT_RETRIEVE)
Laurent@2111:                         publish_template = variable_infos.get("publish", DEFAULT_PUBLISH)
Laurent@2111:                         
Laurent@2111:                         if retrieve_template is not None:
Laurent@2111:                             str_completion["extra_variables_retrieve"].append(
Laurent@2111:                                 retrieve_template % var_infos)
Laurent@2111:                         if publish_template is not None:
Laurent@2111:                             str_completion["extra_variables_publish"].append(
Laurent@2111:                                 publish_template % var_infos)
Laurent@2111:             elif param["value"] is not None:
Laurent@2111:                 param_infos = {
Laurent@2111:                     "location": location_str,
Laurent@2111:                     "param_name": param["name"],
Laurent@2111:                 }
Laurent@2111:                 if param["type"] == "boolean":
Laurent@2111:                     param_infos["param_value"] = {True: "true", False: "false"}[param["value"]]
Laurent@2111:                 else:
Laurent@2111:                     param_infos["param_value"] = str(param["value"])
Laurent@2111:                 str_completion["init_axis_params"].append(
Laurent@2111:                     "        __CIA402Node_%(location)s.axis->%(param_name)s = %(param_value)s;" % param_infos)
Laurent@2111:         
Laurent@2111:         for variable in variables:
Laurent@2111:             var_infos = dict(zip(["name", "index", "subindex", "var_type", "dir"], variable))
Laurent@2111:             var_infos["location"] = location_str
Laurent@2111:             var_infos["var_size"] = self.GetSizeOfType(var_infos["var_type"])
Laurent@2111:             var_infos["var_name"] = "__%(dir)s%(var_size)s%(location)s_%(index)d_%(subindex)d" % var_infos
Laurent@2111:             
Laurent@2111:             str_completion["extern_located_variables_declaration"].append(
Laurent@2111:                     "IEC_%(var_type)s *%(var_name)s;" % var_infos)
Laurent@2111:             str_completion["entry_variables"].append(
Laurent@2111:                     "    IEC_%(var_type)s *%(name)s;" % var_infos)
Laurent@2111:             str_completion["init_entry_variables"].append(
Laurent@2111:                     "    __CIA402Node_%(location)s.%(name)s = %(var_name)s;" % var_infos)
Laurent@2111:             
Laurent@2111:             self.CTNParent.FileGenerator.DeclareVariable(
Laurent@2111:                     self.GetSlavePos(), var_infos["index"], var_infos["subindex"], 
Laurent@2111:                     var_infos["var_type"], var_infos["dir"], var_infos["var_name"])
Laurent@2111:         
Laurent@2111:         for element in ["extern_located_variables_declaration", 
Laurent@2111:                         "fieldbus_interface_declaration",
Laurent@2111:                         "fieldbus_interface_definition",
Laurent@2111:                         "entry_variables", 
Laurent@2111:                         "init_axis_params", 
Laurent@2111:                         "init_entry_variables",
Laurent@2111:                         "extra_variables_retrieve",
Laurent@2111:                         "extra_variables_publish"]:
Laurent@2111:             str_completion[element] = "\n".join(str_completion[element])
Laurent@2111:         
Laurent@2111:         Gen_CIA402Nodefile_path = os.path.join(buildpath, "cia402node_%s.c"%location_str)
Laurent@2111:         cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')
Laurent@2111:         cia402nodefile.write(plc_cia402node_code % str_completion)
Laurent@2111:         cia402nodefile.close()
Laurent@2111:         
Laurent@2111:         return [(Gen_CIA402Nodefile_path, '"-I%s"'%os.path.abspath(self.GetCTRoot().GetIECLibPath()))],"",True