diff -r 09d5d1456616 -r c9deff128c37 etherlab/EthercatCIA402Slave.py --- a/etherlab/EthercatCIA402Slave.py Sat Jun 23 09:17:20 2018 +0200 +++ b/etherlab/EthercatCIA402Slave.py Wed Nov 20 16:57:15 2019 +0100 @@ -13,28 +13,35 @@ import wx -from PLCControler import LOCATION_CONFNODE, LOCATION_MODULE, LOCATION_GROUP, LOCATION_VAR_INPUT, LOCATION_VAR_OUTPUT, LOCATION_VAR_MEMORY +from PLCControler import LOCATION_CONFNODE, LOCATION_MODULE, LOCATION_GROUP, \ + LOCATION_VAR_INPUT, LOCATION_VAR_OUTPUT, LOCATION_VAR_MEMORY from MotionLibrary import Headers, AxisXSD from EthercatSlave import _EthercatSlaveCTN, _CommonSlave from ConfigEditor import CIA402NodeEditor -# Definition of node variables that have to be mapped in PDO -# [(name, index, subindex, type, -# direction for master ('I': input, 'Q': output)),...] +#HSAHN 2015.07.26 +#remove NODE_VARIABLES's optional items. +#As for now, optional items will be added dynamicaly. NODE_VARIABLES = [ - ("ControlWord", 0x6040, 0x00, "UINT", "Q"), - ("TargetPosition", 0x607a, 0x00, "DINT", "Q"), - ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"), - ("TargetTorque", 0x6071, 0x00, "INT", "Q"), - ("ModesOfOperation", 0x6060, 0x00, "SINT", "Q"), - ("StatusWord", 0x6041, 0x00, "UINT", "I"), + ("ControlWord", 0x6040, 0x00, "UINT", "Q"), + ("ModesOfOperation", 0x6060, 0x00, "SINT", "Q"), + ("StatusWord", 0x6041, 0x00, "UINT", "I"), ("ModesOfOperationDisplay", 0x6061, 0x00, "SINT", "I"), - ("ActualPosition", 0x6064, 0x00, "DINT", "I"), - ("ActualVelocity", 0x606c, 0x00, "DINT", "I"), - ("ActualTorque", 0x6077, 0x00, "INT", "I"), ] +#HSAHN 2015.07.26 +#reference variable +ADD_NODE_VARIABLES = ({'name':"TargetPosition" , 'index':0x607a, 'sub-index':0x00, 'type':"DINT", 'direction':"Q"}, + {'name':"TargetVelocity" , 'index':0x60ff, 'sub-index':0x00, 'type':"DINT", 'direction':"Q"}, + {'name':"TargetTorque" , 'index':0x6071, 'sub-index':0x00, 'type':"INT", 'direction':"Q"}, + {'name':"ActualPosition" , 'index':0x6064, 'sub-index':0x00, 'type':"DINT", 'direction':"I"}, + {'name':"ActualVelocity" , 'index':0x606c, 'sub-index':0x00, 'type':"DINT", 'direction':"I"}, + {'name':"ActualTorque" , 'index':0x6077, 'sub-index':0x00, 'type':"INT", 'direction':"I"}) + +DEFAULT_RETRIEVE = " __CIA402Node_%(location)s.axis->%(name)s = *(__CIA402Node_%(location)s.%(name)s);" +DEFAULT_PUBLISH = " *(__CIA402Node_%(location)s.%(name)s) = __CIA402Node_%(location)s.axis->%(name)s;" + # Definition of optional node variables that can be added to PDO mapping. # A checkbox will be displayed for each section in node configuration panel to # enable them @@ -46,6 +53,7 @@ # 'publish', string_template_for_publish_variable (None: not published, # default string template if not defined), # },...] + EXTRA_NODE_VARIABLES = [ ("ErrorCode", [ {"description": ("ErrorCode", 0x603F, 0x00, "UINT", "I"), @@ -56,8 +64,8 @@ "publish": None} ]), ("DigitalOutputs", [ - {"description": ("DigitalOutputs", 0x60FE, 0x00, "UDINT", "Q"), - "retrieve": None} + {"description": ("DigitalOutputs", 0x60FE, 0x01, "UDINT", "I"), + "publish": None} ]), ("TouchProbe", [ {"description": ("TouchProbeFunction", 0x60B8, 0x00, "UINT", "Q"), @@ -77,7 +85,23 @@ "Enable" + name: params for name, params in EXTRA_NODE_VARIABLES} -# List of block to define to interface MCL to fieldbus for specific functions +BLOCK_INPUT_TEMPLATE = " __SET_VAR(%(blockname)s->,%(input_name)s, %(input_value)s);" +BLOCK_OUTPUT_TEMPLATE = " __SET_VAR(data__->,%(output_name)s, __GET_VAR(%(blockname)s->%(output_name)s));" + +BLOCK_FUNCTION_TEMPLATE = """ +extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__); +void __%(blocktype)s_%(location)s(MC_%(ucase_blocktype)s *data__) { +__DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s); +ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s(); +__SET_VAR(%(blockname)s->, POS, AxsPub.axis->NetworkPosition); +%(extract_inputs)s +ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s); +%(return_outputs)s +} +""" + +BLOCK_FUNTION_DEFINITION_TEMPLATE = " __CIA402Node_%(location)s.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location)s;" + FIELDBUS_INTERFACE_GLOBAL_INSTANCES = [ {"blocktype": "GetTorqueLimit", "inputs": [], @@ -89,6 +113,44 @@ "outputs": []}, ] +# add jblee +MODEOFOP_HOMING_METHOD_TEMPLATE = """ + if(*(AxsPub.ModesOfOperation) == 0x06){ + IEC_BOOL homing = AxsPub.axis->HomingOperationStart; + if(power){ + if (homing) + CW |= Homing_OperationStart_Origin; + else + CW &= ~(Homing_OperationStart_Origin); + } + else{ + if (homing) + CW |= Homing_OperationStart_Edit; + else + CW &= ~(EnableOperation); + } + + } +""" + +MODEOFOP_COMPUTATION_MODE_TEMPLATE = """ + switch (AxsPub.axis->AxisMotionMode) { + //ssh_add + case mc_mode_hm: + *(AxsPub.ModesOfOperation) = 0x06; + break; + case mc_mode_cst: + *(AxsPub.ModesOfOperation) = 0x0a; + break; + case mc_mode_csv: + *(AxsPub.ModesOfOperation) = 0x09; + break; + default: + *(AxsPub.ModesOfOperation) = 0x08; + break; + } +""" + #-------------------------------------------------- # Ethercat CIA402 Node #-------------------------------------------------- @@ -131,6 +193,13 @@ # ----------- call ethercat mng. function -------------- self.CommonMethod = _CommonSlave(self) + #HSAHN + # Select PDO Mapping + self.SelectedPDOIndex = [] + self.SelectedRxPDOIndex = [] + self.SelectedTxPDOIndex = [] + #HSAHN END + def GetIconName(self): return "CIA402Slave" @@ -186,147 +255,274 @@ self.StartDragNDrop( ("%%IW%s.402" % ".".join(map(str, self.GetCurrentLocation())), "location", "AXIS_REF", self.CTNName(), "")) + + # add jblee + """ + def LoadPDOSelectData(self): + ReadData = [] + files = os.listdir(self.CTNPath()) + filepath = os.path.join(self.CTNPath(), "DataForPDO.txt") + if os.path.isfile(filepath): + PDODataRead = open(filepath, 'r') + ReadData = PDODataRead.readlines() + PDODataRead.close() + + if len(ReadData) > 1: + for data in ReadData[0].split() : + if data == "RxPDO": + continue + self.SelectedRxPDOIndex.append(int(data, 0)) + + for data in ReadData[1].split() : + if data == "TxPDO": + continue + self.SelectedTxPDOIndex.append(int(data, 0)) + """ + + def LoadPDOSelectData(self): + RxPDOData = self.BaseParams.getRxPDO() + RxPDOs = [] + if RxPDOData != "None": + RxPDOs = RxPDOData.split() + if RxPDOs : + for RxPDO in RxPDOs : + self.SelectedRxPDOIndex.append(int(RxPDO, 0)) + + TxPDOData = self.BaseParams.getTxPDO() + TxPDOs = [] + if TxPDOData != "None": + TxPDOs = TxPDOData.split() + if TxPDOs : + for TxPDO in TxPDOs : + self.SelectedTxPDOIndex.append(int(TxPDO, 0)) + + def LoadDefaultPDOSet(self): + ReturnData = [] + rx_pdo_entries = self.CommonMethod.GetRxPDOCategory() + if len(rx_pdo_entries): + for i in range(len(rx_pdo_entries)): + if rx_pdo_entries[i]['sm'] is not None: + ReturnData.append(rx_pdo_entries[i]['pdo_index']) + + tx_pdo_entries = self.CommonMethod.GetTxPDOCategory() + if len(tx_pdo_entries): + for i in range(len(tx_pdo_entries)): + if tx_pdo_entries[i]['sm'] is not None: + ReturnData.append(tx_pdo_entries[i]['pdo_index']) + + if ReturnData : + return ReturnData + else : + return [5632, 6656] def CTNGenerate_C(self, buildpath, locations): current_location = self.GetCurrentLocation() location_str = "_".join(map(lambda x:str(x), current_location)) - slave_pos = self.GetSlavePos() - MCL_headers = Headers - - # Open CIA402 node code template file - plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], - "plc_cia402node.c") + + plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], "plc_cia402node.c") plc_cia402node_file = open(plc_cia402node_filepath, 'r') plc_cia402node_code = plc_cia402node_file.read() plc_cia402node_file.close() - - # Init list of generated strings for each code template file section - fieldbus_interface_declaration = [] - fieldbus_interface_definition = [] - init_axis_params = [] - extra_variables_retrieve = [] - extra_variables_publish = [] - extern_located_variables_declaration = [] - entry_variables = [] - init_entry_variables = [] - - # Fieldbus interface code sections + # HSAHN 150726 + # add "default_variables_retrieve": [], "default_variables_publish": [], + # As PDO mapping object, it will add auto-complete code. + # add "modeofop_homing_method", "modeofop_computation_mode" by jblee + str_completion = { + "slave_pos": self.GetSlavePos(), + "location": location_str, + "MCL_headers": Headers, + "extern_located_variables_declaration": [], + "fieldbus_interface_declaration": [], + "fieldbus_interface_definition": [], + "entry_variables": [], + "init_axis_params": [], + "init_entry_variables": [], + "default_variables_retrieve": [], + "default_variables_publish": [], + "extra_variables_retrieve": [], + "extra_variables_publish": [], + "modeofop_homing_method": [], + "modeofop_computation_mode": [] + } + for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES: - blocktype = blocktype_infos["blocktype"] - ucase_blocktype = blocktype.upper() - blockname = "_".join([ucase_blocktype, location_str]) + texts = { + "blocktype": blocktype_infos["blocktype"], + "ucase_blocktype": blocktype_infos["blocktype"].upper(), + "location": "_".join(map(str, current_location)) + } + texts["blockname"] = "%(ucase_blocktype)s_%(location)s" % texts - extract_inputs = "\n".join(["""\ - __SET_VAR(%s->, %s,, %s);""" % (blockname, input_name, input_value) - for (input_name, input_value) in [ - ("EXECUTE", "__GET_VAR(data__->EXECUTE)")] + [ - (input["name"].upper(), - "__GET_VAR(data__->%s)" % input["name"].upper()) - for input in blocktype_infos["inputs"]] - ]) + inputs = [{"input_name": "POS", "input_value": str(self.GetSlavePos())}, + {"input_name": "EXECUTE", "input_value": "__GET_VAR(data__->EXECUTE)"}] +\ + [{"input_name": input["name"].upper(), + "input_value": "__GET_VAR(data__->%s)" % input["name"].upper()} + for input in blocktype_infos["inputs"]] + input_texts = [] + for input_infos in inputs: + input_infos.update(texts) + input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos) + texts["extract_inputs"] = "\n".join(input_texts) + outputs = [{"output_name": output} for output in ["DONE", "BUSY", "ERROR"]] + \ + [{"output_name": output["name"].upper()} for output in blocktype_infos["outputs"]] + output_texts = [] + for output_infos in outputs: + output_infos.update(texts) + output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos) + texts["return_outputs"] = "\n".join(output_texts) - return_outputs = "\n".join(["""\ - __SET_VAR(data__->,%(output_name)s,, - __GET_VAR(%(blockname)s->%(output_name)s));""" % locals() - for output_name in ["DONE", "BUSY", "ERROR"] + [ - output["name"].upper() - for output in blocktype_infos["outputs"]] - ]) + str_completion["fieldbus_interface_declaration"].append( + BLOCK_FUNCTION_TEMPLATE % texts) + + str_completion["fieldbus_interface_definition"].append( + BLOCK_FUNTION_DEFINITION_TEMPLATE % texts) + + variables = NODE_VARIABLES[:] + +#HSAHN +#2015. 7. 24 PDO Variable + #if PDO is not selected, use 1st PDO set + self.LoadPDOSelectData() + if not self.SelectedRxPDOIndex and not self.SelectedTxPDOIndex : + self.SelectedPDOIndex = self.LoadDefaultPDOSet() + else : + self.SelectedPDOIndex = self.SelectedRxPDOIndex + self.SelectedTxPDOIndex + + add_idx = [] + for i in range(len(ADD_NODE_VARIABLES)): + add_idx.append(ADD_NODE_VARIABLES[i]['index']) + + self.CommonMethod.RequestPDOInfo() + pdo_info = self.CommonMethod.GetRxPDOCategory() + self.CommonMethod.GetTxPDOCategory() + pdo_entry = self.CommonMethod.GetRxPDOInfo() + self.CommonMethod.GetTxPDOInfo() + list_index = 0 + ModeOfOpFlag = False + ModeOfOpDisplayFlag = False + for i in range(len(pdo_info)): + #if pdo_index is in the SelectedPDOIndex: put the PDO mapping information intto the "used" object + if pdo_info[i]['pdo_index'] in self.SelectedPDOIndex: + used = pdo_entry[list_index:list_index + pdo_info[i]['number_of_entry']] + for used_data in used: + # 24672 -> 0x6060, Mode of Operation + if used_data['entry_index'] == 24672: + ModeOfOpFlag = True + # 24673 -> 0x6061, Mode of Operation Display + elif used_data["entry_index"] == 24673: + ModeOfOpDisplayFlag = True + + if used_data['entry_index'] in add_idx: + idx = add_idx.index(used_data['entry_index']) + adder = list([ADD_NODE_VARIABLES[idx]['name'], ADD_NODE_VARIABLES[idx]['index'], \ + ADD_NODE_VARIABLES[idx]['sub-index'], ADD_NODE_VARIABLES[idx]['type'], \ + ADD_NODE_VARIABLES[idx]['direction']]) + variables.append(adder) + if ADD_NODE_VARIABLES[idx]['direction'] == "Q": + parsed_string = ADD_NODE_VARIABLES[idx]['name'].replace("Target", "") + # add jblee + check_q_data = " *(AxsPub.Target%s) = AxsPub.axis->Raw%sSetPoint;" %(parsed_string, parsed_string) + if check_q_data not in str_completion["default_variables_publish"]: + str_completion["default_variables_publish"].append(check_q_data) + elif ADD_NODE_VARIABLES[idx]['direction'] == "I": + parsed_string = ADD_NODE_VARIABLES[idx]['name'].replace("Actual", "") + # add jblee + check_i_data = " AxsPub.axis->ActualRaw%s = *(AxsPub.Actual%s);" %(parsed_string, parsed_string) + if check_i_data not in str_completion["default_variables_retrieve"]: + str_completion["default_variables_retrieve"].append(check_i_data) + list_index += pdo_info[i]['number_of_entry'] +#HSAHN END + + params = self.CTNParams[1].getElementInfos(self.CTNParams[0]) + for param in params["children"]: + if param["name"] in EXTRA_NODE_VARIABLES_DICT: + if param["value"]: + extra_variables = EXTRA_NODE_VARIABLES_DICT.get(param["name"]) + for variable_infos in extra_variables: + var_infos = { + "location": location_str, + "name": variable_infos["description"][0] + } + variables.append(variable_infos["description"]) + retrieve_template = variable_infos.get("retrieve", DEFAULT_RETRIEVE) + publish_template = variable_infos.get("publish", DEFAULT_PUBLISH) - fieldbus_interface_declaration.append(""" -extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__); -void __%(blocktype)s_%(location_str)s(MC_%(ucase_blocktype)s *data__) { -__DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s); -ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s(); -__SET_VAR(%(blockname)s->, POS,, AxsPub.axis->NetworkPosition); -%(extract_inputs)s -ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s); -%(return_outputs)s -}""" % locals()) - - fieldbus_interface_definition.append("""\ - AxsPub.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location_str)s;\ -""" % locals()) - - # Get a copy list of default variables to map - variables = NODE_VARIABLES[:] - - # Set AxisRef public struct members value - node_params = self.CTNParams[1].getElementInfos(self.CTNParams[0]) - for param in node_params["children"]: - param_name = param["name"] - - # Param is optional variables section enable flag - extra_node_variable_infos = EXTRA_NODE_VARIABLES_DICT.get(param_name) - if extra_node_variable_infos is not None: - param_name = param_name.replace("Enable", "") + "Enabled" - - if not param["value"]: - continue - - # Optional variables section is enabled - for variable_infos in extra_node_variable_infos: - var_name = variable_infos["description"][0] - - # Add each variables defined in section description to the - # list of variables to map - variables.append(variable_infos["description"]) - - # Add code to publish or retrive variable - for var_exchange_dir, str_list, default_template in [ - ("retrieve", extra_variables_retrieve, - " AxsPub.axis->%(var_name)s = *(AxsPub.%(var_name)s);"), - ("publish", extra_variables_publish, - " *(AxsPub.%(var_name)s) = AxsPub.axis->%(var_name)s;")]: - - template = variable_infos.get(var_exchange_dir, - default_template) - if template is not None: - extra_variables_publish.append(template % locals()) - - # Set AxisRef public struct member value if defined + if retrieve_template is not None: + str_completion["extra_variables_retrieve"].append( + retrieve_template % var_infos) + if publish_template is not None: + str_completion["extra_variables_publish"].append( + publish_template % var_infos) + + #elif param["value"] is not None: if param["value"] is not None: - param_value = ({True: "1", False: "0"}[param["value"]] - if param["type"] == "boolean" - else str(param["value"])) - - init_axis_params.append("""\ - AxsPub.axis->%(param_name)s = %(param_value)s;""" % locals()) - - # Add each variable in list of variables to map to master list of - # variables to add to network configuration - for name, index, subindex, var_type, dir in variables: - var_size = self.GetSizeOfType(var_type) - var_name = """\ -__%(dir)s%(var_size)s%(location_str)s_%(index)d_%(subindex)d""" % locals() - - extern_located_variables_declaration.append( - "IEC_%(var_type)s *%(var_name)s;" % locals()) - entry_variables.append( - " IEC_%(var_type)s *%(name)s;" % locals()) - init_entry_variables.append( - " AxsPub.%(name)s = %(var_name)s;" % locals()) + param_infos = { + "location": location_str, + "param_name": param["name"], + } + if param["type"] == "boolean": + param_infos["param_value"] = {True: "1", False: "0"}[param["value"]] + param_infos["param_name"] = param["name"].replace("Enable", "") + "Enabled" + if param["value"] == False: + continue + else: + param_infos["param_value"] = str(param["value"]) + # param_name = param_name.replace("Enable", "") + "Enabled" + str_completion["init_axis_params"].append( + " __CIA402Node_%(location)s.axis->%(param_name)s = %(param_value)s;" % param_infos) + + check_variable = [] + for variable in variables: + # add jblee + if variable in check_variable: + continue + + var_infos = dict(zip(["name", "index", "subindex", "var_type", "dir"], variable)) + var_infos["location"] = location_str + var_infos["var_size"] = self.GetSizeOfType(var_infos["var_type"]) + var_infos["var_name"] = "__%(dir)s%(var_size)s%(location)s_%(index)d_%(subindex)d" % var_infos + + # add jblee + if var_infos["index"] in [24672] and ModeOfOpFlag: + str_completion["modeofop_homing_method"].append(MODEOFOP_HOMING_METHOD_TEMPLATE) + str_completion["modeofop_computation_mode"].append(MODEOFOP_COMPUTATION_MODE_TEMPLATE) + + # add jblee + if var_infos["index"] in [24672, 24673] and (not ModeOfOpFlag or not ModeOfOpDisplayFlag): + continue + + str_completion["extern_located_variables_declaration"].append( + "IEC_%(var_type)s *%(var_name)s;" % var_infos) + str_completion["entry_variables"].append( + " IEC_%(var_type)s *%(name)s;" % var_infos) + str_completion["init_entry_variables"].append( + " __CIA402Node_%(location)s.%(name)s = %(var_name)s;" % var_infos) self.CTNParent.FileGenerator.DeclareVariable( - slave_pos, index, subindex, var_type, dir, var_name) - - # Add newline between string in list of generated strings for sections - [fieldbus_interface_declaration, fieldbus_interface_definition, - init_axis_params, extra_variables_retrieve, extra_variables_publish, - extern_located_variables_declaration, entry_variables, - init_entry_variables] = map(lambda l: "\n".join(l), [ - fieldbus_interface_declaration, fieldbus_interface_definition, - init_axis_params, extra_variables_retrieve, extra_variables_publish, - extern_located_variables_declaration, entry_variables, - init_entry_variables]) - - # Write generated content to CIA402 node file - Gen_CIA402Nodefile_path = os.path.join(buildpath, - "cia402node_%s.c"%location_str) + self.GetSlavePos(), var_infos["index"], var_infos["subindex"], + var_infos["var_type"], var_infos["dir"], var_infos["var_name"]) + + # add jblee + check_variable.append(variable) + + for element in ["extern_located_variables_declaration", + "fieldbus_interface_declaration", + "fieldbus_interface_definition", + "entry_variables", + "init_axis_params", + "init_entry_variables", + "default_variables_retrieve", + "default_variables_publish", + "extra_variables_retrieve", + "extra_variables_publish", + "modeofop_homing_method", + "modeofop_computation_mode"]: + str_completion[element] = "\n".join(str_completion[element]) + + Gen_CIA402Nodefile_path = os.path.join(buildpath, "cia402node_%s.c"%location_str) cia402nodefile = open(Gen_CIA402Nodefile_path, 'w') - cia402nodefile.write(plc_cia402node_code % locals()) + cia402nodefile.write(plc_cia402node_code % str_completion) cia402nodefile.close() return [(Gen_CIA402Nodefile_path, '"-I%s"'%os.path.abspath(self.GetCTRoot().GetIECLibPath()))],"",True +