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

from __future__ import absolute_import

    13

import os

    14

    15

import wx

    16

    17

from PLCControler import LOCATION_CONFNODE, LOCATION_VAR_INPUT

    18

    19

from MotionLibrary import AxisXSD

    20

from etherlab.EthercatSlave import _EthercatSlaveCTN, _CommonSlave

    21

from etherlab.ConfigEditor import CIA402NodeEditor

    22

    23

# Definition of node variables that have to be mapped in PDO

    24

# [(name, index, subindex, type,

    25

#   direction for master ('I': input, 'Q': output)),...]

    26

NODE_VARIABLES = [

    27

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

    28

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

    29

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

    30

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

    31

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

    32

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

    33

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

    34

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

    35

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

    36

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

    37

]

    38

    39

# Definition of optional node variables that can be added to PDO mapping.

    40

# A checkbox will be displayed for each section in node configuration panel to

    41

# enable them

    42

# [(section_name,

    43

#   [{'description', (name, index, subindex, type,

    44

#                     direction for master ('I': input, 'Q': output)),

    45

#     'retrieve', string_template_for_retrieve_variable (None: not retrieved,

    46

#                                 default string template if not defined),

    47

#     'publish', string_template_for_publish_variable (None: not published,

    48

#                                 default string template if not defined),

    49

#    },...]

    50

EXTRA_NODE_VARIABLES = [

    51

    ("ErrorCode", [

    52

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

    53

         "publish": None}

    54

        ]),

    55

    ("DigitalInputs", [

    56

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

    57

         "publish": None}

    58

        ]),

    59

    ("DigitalOutputs", [

    60

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

    61

         "retrieve": None}

    62

        ]),

    63

    ("TouchProbe", [

    64

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

    65

         "retrieve": None},

    66

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

    67

         "publish": None},

    68

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

    69

         "publish": None},

    70

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

    71

         "publish": None},

    72

        ]),

    73

]

    74

    75

# List of parameters name in no configuration panel for optional variable

    76

# sections

    77

EXTRA_NODE_VARIABLES_DICT = {

    78

    "Enable" + name: params

    79

    for name, params in EXTRA_NODE_VARIABLES}

    80

    81

# List of block to define to interface MCL to fieldbus for specific functions

    82

FIELDBUS_INTERFACE_GLOBAL_INSTANCES = [

    83

    {"blocktype": "GetTorqueLimit",

    84

     "inputs": [],

    85

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

    86

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

    87

    {"blocktype": "SetTorqueLimit",

    88

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

    89

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

    90

     "outputs": []},

    91

]

    92

    93

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

    94

#                 Ethercat CIA402 Node

    95

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

    96

    97

    98

class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):

    99

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

   100

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

   101

      <xsd:element name="CIA402SlaveParams">

   102

        <xsd:complexType>

   103

          %s

   104

        </xsd:complexType>

   105

      </xsd:element>

   106

    </xsd:schema>

   107

    """ % ("\n".join(["""\

   108

          <xsd:attribute name="Enable%s" type="xsd:boolean"

   109

                         use="optional" default="false"/>""" % category

   110

                      for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)

   111

   112

    NODE_PROFILE = 402

   113

    EditorType = CIA402NodeEditor

   114

   115

    ConfNodeMethods = [

   116

        {

   117

            "bitmap": "CIA402AxisRef",

   118

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

   119

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

   120

            "method": "_getCIA402AxisRef",

   121

            "push": True,

   122

        },

   123

        {

   124

            "bitmap": "CIA402NetPos",

   125

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

   126

            "tooltip": _("Initiate Drag'n drop of Network position located variable"),

   127

            "method": "_getCIA402NetworkPosition",

   128

            "push": True,

   129

        },

   130

    ]

   131

   132

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

   133

#    class code

   134

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

   135

   136

    def __init__(self):

   137

        _EthercatSlaveCTN.__init__(self)

   138

   139

        # ----------- call ethercat mng. function --------------

   140

        self.CommonMethod = _CommonSlave(self)

   141

   142

    def GetIconName(self):

   143

        return "CIA402Slave"

   144

   145

    def SetParamsAttribute(self, path, value):

   146

        if path == "CIA402SlaveParams.Type":

   147

            path = "SlaveParams.Type"

   148

        elif path == "CIA402SlaveParams.Alias":

   149

            path = "SlaveParams.Alias"

   150

        return _EthercatSlaveCTN.SetParamsAttribute(self, path, value)

   151

   152

    def GetVariableLocationTree(self):

   153

        axis_name = self.CTNName()

   154

        current_location = self.GetCurrentLocation()

   155

        children = [

   156

            {

   157

                "name": name_frmt % (axis_name),

   158

                "type": LOCATION_VAR_INPUT,

   159

                "size": "W",

   160

                "IEC_type": iec_type,

   161

                "var_name": var_name_frmt % axis_name,

   162

                "location": location_frmt % (".".join(map(str, current_location))),

   163

                "description": "",

   164

                "children": []

   165

            }

   166

            for name_frmt, iec_type, var_name_frmt, location_frmt in [

   167

                ("%s Network Position", "UINT", "%s_pos", "%%IW%s"),

   168

                ("%s Axis Ref", "AXIS_REF", "%s", "%%IW%s.402")

   169

            ]

   170

        ]

   171

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

   172

                                                             current_location,

   173

                                                             axis_name))

   174

        return {

   175

            "name": axis_name,

   176

            "type": LOCATION_CONFNODE,

   177

            "location": self.GetFullIEC_Channel(),

   178

            "children": children,

   179

        }

   180

   181

    def CTNGlobalInstances(self):

   182

        current_location = self.GetCurrentLocation()

   183

        return [("%s_%s" % (block_infos["blocktype"],

   184

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

   185

                 "EtherLab%s" % block_infos["blocktype"], "")

   186

                for block_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES]

   187

   188

    def StartDragNDrop(self, data):

   189

        data_obj = wx.TextDataObject(str(data))

   190

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

   191

        dragSource.SetData(data_obj)

   192

        dragSource.DoDragDrop()

   193

   194

    def _getCIA402NetworkPosition(self):

   195

        self.StartDragNDrop(

   196

            ("%%IW%s" % ".".join(map(str, self.GetCurrentLocation())),

   197

             "location", "UINT", self.CTNName() + "_Pos", ""))

   198

   199

    def _getCIA402AxisRef(self):

   200

        self.StartDragNDrop(

   201

            ("%%IW%s.402" % ".".join(map(str, self.GetCurrentLocation())),

   202

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

   203

   204

    def CTNGenerate_C(self, buildpath, locations):

   205

        current_location = self.GetCurrentLocation()

   206

   207

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

   208

        slave_pos = self.GetSlavePos()

   209

   210

        # Open CIA402 node code template file

   211

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

   212

                                               "plc_cia402node.c")

   213

        plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   214

        plc_cia402node_code = plc_cia402node_file.read()

   215

        plc_cia402node_file.close()

   216

   217

        # Init list of generated strings for each code template file section

   218

        fieldbus_interface_declaration = []

   219

        fieldbus_interface_definition = []

   220

        init_axis_params = []

   221

        extra_variables_retrieve = []

   222

        extra_variables_publish = []

   223

        extern_located_variables_declaration = []

   224

        entry_variables = []

   225

        init_entry_variables = []

   226

   227

        # Fieldbus interface code sections

   228

        for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES:

   229

            blocktype = blocktype_infos["blocktype"]

   230

            ucase_blocktype = blocktype.upper()

   231

            blockname = "_".join([ucase_blocktype, location_str])

   232

   233

            extract_inputs = "\n".join([

   234

                """\

   235

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

   236

                for (input_name, input_value) in

   237

                [("EXECUTE", "__GET_VAR(data__->EXECUTE)")] + [

   238

                    (input["name"].upper(),

   239

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

   240

                    for input in blocktype_infos["inputs"]

   241

                ]])

   242

   243

            return_outputs = "\n".join([

   244

                """\

   245

                __SET_VAR(data__->,%(output_name)s,,

   246

                __GET_VAR(%(blockname)s->%(output_name)s));""" % {

   247

                    "output_name": output_name,

   248

                    "blockname": blockname

   249

                }

   250

                for output_name in ["DONE", "BUSY", "ERROR"] + [

   251

                    output["name"].upper()

   252

                    for output in blocktype_infos["outputs"]]

   253

            ])

   254

   255

            loc_dict = {

   256

                "ucase_blocktype": ucase_blocktype,

   257

                "blocktype": blocktype,

   258

                "blockname": blockname,

   259

                "location_str": location_str,

   260

                "extract_inputs": extract_inputs,

   261

                "return_outputs": return_outputs,

   262

            }

   263

   264

            fieldbus_interface_declaration.append("""

   265

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

   266

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

   267

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

   268

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

   269

__SET_VAR(%(blockname)s->, POS,, AxsPub.axis->NetworkPosition);

   270

%(extract_inputs)s

   271

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

   272

%(return_outputs)s

   273

}""" % loc_dict)

   274

   275

            fieldbus_interface_definition.append("""\

   276

        AxsPub.axis->__mcl_func_MC_%(blocktype)s = __%(blocktype)s_%(location_str)s;\

   277

""" % loc_dict)

   278

   279

        # Get a copy list of default variables to map

   280

        variables = NODE_VARIABLES[:]

   281

   282

        # Set AxisRef public struct members value

   283

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

   284

        for param in node_params["children"]:

   285

            param_name = param["name"]

   286

   287

            # Param is optional variables section enable flag

   288

            extra_node_variable_infos = EXTRA_NODE_VARIABLES_DICT.get(param_name)

   289

            if extra_node_variable_infos is not None:

   290

                param_name = param_name.replace("Enable", "") + "Enabled"

   291

   292

                if not param["value"]:

   293

                    continue

   294

   295

                # Optional variables section is enabled

   296

                for variable_infos in extra_node_variable_infos:

   297

                    var_name = variable_infos["description"][0]

   298

   299

                    # Add each variables defined in section description to the

   300

                    # list of variables to map

   301

                    variables.append(variable_infos["description"])

   302

   303

                    # Add code to publish or retrive variable

   304

                    coded = [

   305

                        ("retrieve",

   306

                         extra_variables_retrieve,

   307

                         "    AxsPub.axis->%(var_name)s = *(AxsPub.%(var_name)s);"),

   308

                        ("publish",

   309

                         extra_variables_publish,

   310

                         "    *(AxsPub.%(var_name)s) = AxsPub.axis->%(var_name)s;")

   311

                    ]

   312

                    for var_exchange_dir, _str_list, default_template in coded:

   313

                        template = variable_infos.get(var_exchange_dir, default_template)

   314

                        if template is not None:

   315

                            extra_variables_publish.append(template % locals())

   316

   317

            # Set AxisRef public struct member value if defined

   318

            if param["value"] is not None:

   319

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

   320

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

   321

                               else str(param["value"]))

   322

   323

                init_axis_params.append("""\

   324

                AxsPub.axis->%(param_name)s = %(param_value)s;""" % {

   325

                    "param_value": param_value,

   326

                    "param_name": param_name,

   327

                })

   328

   329

        # Add each variable in list of variables to map to master list of

   330

        # variables to add to network configuration

   331

        for name, index, subindex, var_type, dir in variables:

   332

            var_size = self.GetSizeOfType(var_type)

   333

            loc_dict = {

   334

                "var_size": var_size,

   335

                "var_type": var_type,

   336

                "name:": name,

   337

                "location_str": location_str,

   338

                "index": index,

   339

                "subindex": subindex,

   340

            }

   341

            var_name = """\

   342

__%(dir)s%(var_size)s%(location_str)s_%(index)d_%(subindex)d""" % loc_dict

   343

            loc_dict["var_name"] = var_name

   344

   345

            extern_located_variables_declaration.append(

   346

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

   347

            entry_variables.append(

   348

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

   349

            init_entry_variables.append(

   350

                "    AxsPub.%(name)s = %(var_name)s;" % loc_dict)

   351

   352

            self.CTNParent.FileGenerator.DeclareVariable(

   353

                slave_pos, index, subindex, var_type, dir, var_name)

   354

   355

        # Add newline between string in list of generated strings for sections

   356

        [fieldbus_interface_declaration, fieldbus_interface_definition,

   357

         init_axis_params, extra_variables_retrieve, extra_variables_publish,

   358

         extern_located_variables_declaration, entry_variables,

   359

         init_entry_variables] = map("\n".join, [

   360

             fieldbus_interface_declaration, fieldbus_interface_definition,

   361

             init_axis_params, extra_variables_retrieve, extra_variables_publish,

   362

             extern_located_variables_declaration, entry_variables,

   363

             init_entry_variables])

   364

   365

        # Write generated content to CIA402 node file

   366

        Gen_CIA402Nodefile_path = os.path.join(buildpath,

   367

                                               "cia402node_%s.c" % location_str)

   368

        cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   369

        cia402nodefile.write(plc_cia402node_code % locals())

   370

        cia402nodefile.close()

   371

   372

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