1

import os

     2

     3

import wx

     4

     5

from PLCControler import LOCATION_CONFNODE, LOCATION_MODULE, LOCATION_GROUP, LOCATION_VAR_INPUT, LOCATION_VAR_OUTPUT, LOCATION_VAR_MEMORY

     6

     7

from MotionLibrary import Headers, AxisXSD

     9

from ConfigEditor import CIA402NodeEditor

    10

    15

NODE_VARIABLES = [

    16

    ("ControlWord", 0x6040, 0x00, "UINT", "Q"),

    17

    ("TargetPosition", 0x607a, 0x00, "DINT", "Q"),

    18

    ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"),

    19

    ("TargetTorque", 0x6071, 0x00, "INT", "Q"),

    20

    ("ModesOfOperation", 0x06060, 0x00, "SINT", "Q"),

    21

    ("StatusWord", 0x6041, 0x00, "UINT", "I"),

    22

    ("ModesOfOperationDisplay", 0x06061, 0x00, "SINT", "I"),

    23

    ("ActualPosition", 0x6064, 0x00, "DINT", "I"),

    24

    ("ActualVelocity", 0x606c, 0x00, "DINT", "I"),

    25

    ("ActualTorque", 0x6077, 0x00, "INT", "I"),

    26

]

    27

    28

DEFAULT_RETRIEVE = "    __CIA402Node_%(location)s.axis->%(name)s = *(__CIA402Node_%(location)s.%(name)s);"

    29

DEFAULT_PUBLISH = "    *(__CIA402Node_%(location)s.%(name)s) = __CIA402Node_%(location)s.axis->%(name)s;"

    30

    31

EXTRA_NODE_VARIABLES = [

    32

    ("ErrorCode", [

    33

        {"description": ("ErrorCode", 0x603F, 0x00, "UINT", "I"),

    34

         "publish": None}

    35

        ]),

    36

    ("DigitalInputs", [

    37

        {"description": ("DigitalInputs", 0x60FD, 0x00, "UDINT", "I"),

    38

         "publish": None}

    39

        ]),

    40

    ("DigitalOutputs", [

    41

        {"description": ("DigitalOutputs", 0x60FE, 0x00, "UDINT", "Q"),

    42

         "retrieve": None}

    43

        ]),

    44

    ("TouchProbe", [

    45

        {"description": ("TouchProbeFunction", 0x60B8, 0x00, "UINT", "Q"),

    46

         "retrieve": None},

    47

        {"description": ("TouchProbeStatus", 0x60B9, 0x00, "UINT", "I"),

    48

         "publish": None},

    49

        {"description": ("TouchProbePos1PosValue", 0x60BA, 0x00, "DINT", "I"),

    50

         "publish": None},

    51

        {"description": ("TouchProbePos1NegValue", 0x60BB, 0x00, "DINT", "I"),

    52

         "publish": None},

    53

        ]),

    54

]

    55

EXTRA_NODE_VARIABLES_DICT = dict([("Enable" + name, value) for name, value in EXTRA_NODE_VARIABLES])

    56

    57

BLOCK_INPUT_TEMPLATE = "    __SET_VAR(%(blockname)s->,%(input_name)s, %(input_value)s);"

    58

BLOCK_OUTPUT_TEMPLATE = "    __SET_VAR(data__->,%(output_name)s, __GET_VAR(%(blockname)s->%(output_name)s));"

    59

    60

BLOCK_FUNCTION_TEMPLATE = """

    61

extern void ETHERLAB%(ucase_blocktype)s_body__(ETHERLAB%(ucase_blocktype)s* data__);

    62

void __%(blocktype)s_%(location)s(MC_%(ucase_blocktype)s *data__) {

    63

__DECLARE_GLOBAL_PROTOTYPE(ETHERLAB%(ucase_blocktype)s, %(blockname)s);

    64

ETHERLAB%(ucase_blocktype)s* %(blockname)s = __GET_GLOBAL_%(blockname)s();

    65

%(extract_inputs)s

    66

ETHERLAB%(ucase_blocktype)s_body__(%(blockname)s);

    67

%(return_outputs)s

    68

}

    69

"""

    70

    71

BLOCK_FUNTION_DEFINITION_TEMPLATE = "        __CIA402Node_%(location)s.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location)s;"

    72

    73

GLOBAL_INSTANCES = [

    74

    {"blocktype": "GetTorqueLimit", 

    75

     "inputs": [],

    76

     "outputs": [{"name": "TorqueLimitPos", "type": "UINT"},

    77

                 {"name": "TorqueLimitNeg", "type": "UINT"}]},

    78

    {"blocktype": "SetTorqueLimit", 

    79

     "inputs": [{"name": "TorqueLimitPos", "type": "UINT"},

    80

                {"name": "TorqueLimitNeg", "type": "UINT"}],

    81

     "outputs": []},

    82

]

    83

    84

#--------------------------------------------------

    85

#                 Ethercat CIA402 Node

    86

#--------------------------------------------------

    87

    88

class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):

    89

    XSD = """<?xml version="1.0" encoding="ISO-8859-1" ?>

    90

    <xsd:schema xmlns:xsd="http://www.w3.org/2001/XMLSchema">

    91

      <xsd:element name="CIA402SlaveParams">

    92

        <xsd:complexType>

    93

          %s

    94

        </xsd:complexType>

    95

      </xsd:element>

    96

    </xsd:schema>

    97

    """ % ("\n".join(['<xsd:attribute name="Enable%s" type="xsd:boolean" use="optional" default="false"/>' % category 

    98

                      for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)

    99

   100

    NODE_PROFILE = 402

   101

    EditorType = CIA402NodeEditor

   102

   103

    ConfNodeMethods = [

   104

        {"bitmap" : "CIA402AxisRef",

   105

         "name" : _("Axis Ref"),

   106

         "tooltip" : _("Initiate Drag'n drop of Axis ref located variable"),

   107

         "method" : "_getCIA402AxisRef",

   108

         "push": True},

   118

   119

    def GetIconName(self):

   120

        return "CIA402Slave"

   121

   122

    def SetParamsAttribute(self, path, value):

   123

        if path == "CIA402SlaveParams.Type":

   124

            path = "SlaveParams.Type"

   125

        elif path == "CIA402SlaveParams.Alias":

   126

            path = "SlaveParams.Alias"

   127

        return _EthercatSlaveCTN.SetParamsAttribute(self, path, value)

   128

   129

    def GetVariableLocationTree(self):

   130

        axis_name = self.CTNName()

   131

        current_location = self.GetCurrentLocation()

   132

        children = [{"name": "%s Axis Ref" % (axis_name),

   133

                     "type": LOCATION_VAR_INPUT,

   134

                     "size": "W",

   135

                     "IEC_type": "AXIS_REF",

   136

                     "var_name": axis_name,

   137

                     "location": "%%IW%s.0" % (".".join(map(str, current_location))),

   138

                     "description": "",

   139

                     "children": []}]

   140

        children.extend(self.CTNParent.GetDeviceLocationTree(self.GetSlavePos(), current_location, axis_name))

   141

        return  {"name": axis_name,

   142

                 "type": LOCATION_CONFNODE,

   143

                 "location": self.GetFullIEC_Channel(),

   144

                 "children": children,

   145

        }

   146

   147

    def CTNGlobalInstances(self):

   148

        current_location = self.GetCurrentLocation()

   149

        return [("%s_%s" % (block_infos["blocktype"], "_".join(map(str, current_location))),

   150

                 "EtherLab%s" % block_infos["blocktype"], "") for block_infos in GLOBAL_INSTANCES]

   151

   152

    def _getCIA402AxisRef(self):

   153

        data = wx.TextDataObject(str(("%%IW%s.0" % ".".join(map(str, self.GetCurrentLocation())), 

   154

                                      "location", "AXIS_REF", self.CTNName(), "")))

   155

        dragSource = wx.DropSource(self.GetCTRoot().AppFrame)

   156

        dragSource.SetData(data)

   157

        dragSource.DoDragDrop()

   158

   159

    def CTNGenerate_C(self, buildpath, locations):

   160

        current_location = self.GetCurrentLocation()

   161

   162

        location_str = "_".join(map(lambda x:str(x), current_location))

   163

   164

        plc_cia402node_filepath = os.path.join(os.path.split(__file__)[0], "plc_cia402node.c")

   165

        plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   166

        plc_cia402node_code = plc_cia402node_file.read()

   167

        plc_cia402node_file.close()

   168

   169

        str_completion = {

   170

            "slave_pos": self.GetSlavePos(),

   171

            "location": location_str,

   172

            "MCL_headers": Headers,

   173

            "extern_located_variables_declaration": [],

   174

            "fieldbus_interface_declaration": [],

   175

            "fieldbus_interface_definition": [],

   176

            "entry_variables": [],

   177

            "init_axis_params": [],

   178

            "init_entry_variables": [],

   179

            "extra_variables_retrieve": [],

   180

            "extra_variables_publish": []

   181

        }

   182

   183

        for blocktype_infos in GLOBAL_INSTANCES:

   184

            texts = {

   185

                "blocktype": blocktype_infos["blocktype"],

   186

                "ucase_blocktype": blocktype_infos["blocktype"].upper(),

   187

                "location": "_".join(map(str, current_location))

   188

            }

   189

            texts["blockname"] = "%(ucase_blocktype)s_%(location)s" % texts

   190

   191

            inputs = [{"input_name": "POS", "input_value": str(self.GetSlavePos())},

   192

                      {"input_name": "EXECUTE", "input_value": "__GET_VAR(data__->EXECUTE)"}] +\

   193

                     [{"input_name": input["name"].upper(), 

   194

                       "input_value": "__GET_VAR(data__->%s)" % input["name"].upper()}

   195

                      for input in blocktype_infos["inputs"]]

   196

            input_texts = []

   197

            for input_infos in inputs:

   198

                input_infos.update(texts)

   199

                input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos)

   200

            texts["extract_inputs"] = "\n".join(input_texts)

   201

   202

            outputs = [{"output_name": output} for output in ["DONE", "BUSY", "ERROR"]] + \

   203

                      [{"output_name": output["name"].upper()} for output in blocktype_infos["outputs"]]

   204

            output_texts = []

   205

            for output_infos in outputs:

   206

                output_infos.update(texts)

   207

                output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos)

   208

            texts["return_outputs"] = "\n".join(output_texts)

   209

   210

            str_completion["fieldbus_interface_declaration"].append(

   211

                    BLOCK_FUNCTION_TEMPLATE % texts)

   212

   213

            str_completion["fieldbus_interface_definition"].append(

   214

                    BLOCK_FUNTION_DEFINITION_TEMPLATE % texts)

   215

   216

        variables = NODE_VARIABLES[:]

   217

   218

        params = self.CTNParams[1].getElementInfos(self.CTNParams[0])

   219

        for param in params["children"]:

   220

            if param["name"] in EXTRA_NODE_VARIABLES_DICT:

   221

                if param["value"]:

   222

                    extra_variables = EXTRA_NODE_VARIABLES_DICT.get(param["name"])

   223

                    for variable_infos in extra_variables:

   224

                        var_infos = {

   225

                            "location": location_str,

   226

                            "name": variable_infos["description"][0]

   227

                        }

   228

                        variables.append(variable_infos["description"])

   229

                        retrieve_template = variable_infos.get("retrieve", DEFAULT_RETRIEVE)

   230

                        publish_template = variable_infos.get("publish", DEFAULT_PUBLISH)

   231

   232

                        if retrieve_template is not None:

   233

                            str_completion["extra_variables_retrieve"].append(

   234

                                retrieve_template % var_infos)

   235

                        if publish_template is not None:

   236

                            str_completion["extra_variables_publish"].append(

   237

                                publish_template % var_infos)

   238

            elif param["value"] is not None:

   239

                param_infos = {

   240

                    "location": location_str,

   241

                    "param_name": param["name"],

   242

                }

   243

                if param["type"] == "boolean":

   244

                    param_infos["param_value"] = {True: "1", False: "0"}[param["value"]]

   245

                else:

   246

                    param_infos["param_value"] = str(param["value"])

   247

                str_completion["init_axis_params"].append(

   248

                    "        __CIA402Node_%(location)s.axis->%(param_name)s = %(param_value)s;" % param_infos)

   249

   250

        for variable in variables:

   251

            var_infos = dict(zip(["name", "index", "subindex", "var_type", "dir"], variable))

   252

            var_infos["location"] = location_str

   253

            var_infos["var_size"] = self.GetSizeOfType(var_infos["var_type"])

   254

            var_infos["var_name"] = "__%(dir)s%(var_size)s%(location)s_%(index)d_%(subindex)d" % var_infos

   255

   256

            str_completion["extern_located_variables_declaration"].append(

   257

                    "IEC_%(var_type)s *%(var_name)s;" % var_infos)

   258

            str_completion["entry_variables"].append(

   259

                    "    IEC_%(var_type)s *%(name)s;" % var_infos)

   260

            str_completion["init_entry_variables"].append(

   261

                    "    __CIA402Node_%(location)s.%(name)s = %(var_name)s;" % var_infos)

   262

   263

            self.CTNParent.FileGenerator.DeclareVariable(

   264

                    self.GetSlavePos(), var_infos["index"], var_infos["subindex"], 

   265

                    var_infos["var_type"], var_infos["dir"], var_infos["var_name"])

   266

   267

        for element in ["extern_located_variables_declaration", 

   268

                        "fieldbus_interface_declaration",

   269

                        "fieldbus_interface_definition",

   270

                        "entry_variables", 

   271

                        "init_axis_params", 

   272

                        "init_entry_variables",

   273

                        "extra_variables_retrieve",

   274

                        "extra_variables_publish"]:

   275

            str_completion[element] = "\n".join(str_completion[element])

   276

   277

        Gen_CIA402Nodefile_path = os.path.join(buildpath, "cia402node_%s.c"%location_str)

   278

        cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   279

        cia402nodefile.write(plc_cia402node_code % str_completion)

   280

        cia402nodefile.close()

   281

   282

        return [(Gen_CIA402Nodefile_path, '"-I%s"'%os.path.abspath(self.GetCTRoot().GetIECLibPath()))],"",True