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

import os

    13

    14

import wx

    15

    16

from PLCControler import LOCATION_CONFNODE, LOCATION_VAR_INPUT

    17

    18

from MotionLibrary import Headers, AxisXSD

    19

from EthercatSlave import _EthercatSlaveCTN, _CommonSlave

    20

from ConfigEditor import CIA402NodeEditor

    21

    22

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

    23

# [(name, index, subindex, type,

    24

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

    25

NODE_VARIABLES = [

    26

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

    27

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

    28

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

    29

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

    30

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

    31

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

    32

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

    33

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

    34

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

    35

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

    36

]

    37

    38

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

    39

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

    40

# enable them

    41

# [(section_name,

    42

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

    43

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

    44

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

    45

#                                 default string template if not defined),

    46

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

    47

#                                 default string template if not defined),

    48

#    },...]

    49

EXTRA_NODE_VARIABLES = [

    50

    ("ErrorCode", [

    51

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

    52

         "publish": None}

    53

        ]),

    54

    ("DigitalInputs", [

    55

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

    56

         "publish": None}

    57

        ]),

    58

    ("DigitalOutputs", [

    59

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

    60

         "retrieve": None}

    61

        ]),

    62

    ("TouchProbe", [

    63

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

    64

         "retrieve": None},

    65

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

    66

         "publish": None},

    67

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

    68

         "publish": None},

    69

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

    70

         "publish": None},

    71

        ]),

    72

]

    73

    74

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

    75

# sections

    76

EXTRA_NODE_VARIABLES_DICT = {

    77

    "Enable" + name: params

    78

    for name, params in EXTRA_NODE_VARIABLES}

    79

    80

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

    81

FIELDBUS_INTERFACE_GLOBAL_INSTANCES = [

    82

    {"blocktype": "GetTorqueLimit",

    83

     "inputs": [],

    84

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

    85

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

    86

    {"blocktype": "SetTorqueLimit",

    87

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

    88

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

    89

     "outputs": []},

    90

]

    91

    92

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

    93

#                 Ethercat CIA402 Node

    94

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

    95

    96

    97

class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):

    98

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

    99

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

   100

      <xsd:element name="CIA402SlaveParams">

   101

        <xsd:complexType>

   102

          %s

   103

        </xsd:complexType>

   104

      </xsd:element>

   105

    </xsd:schema>

   106

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

   107

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

   108

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

   109

                      for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)

   110

   111

    NODE_PROFILE = 402

   112

    EditorType = CIA402NodeEditor

   113

   114

    ConfNodeMethods = [

   115

        {

   116

            "bitmap": "CIA402AxisRef",

   117

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

   118

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

   119

            "method": "_getCIA402AxisRef",

   120

            "push": True,

   121

        },

   122

        {

   123

            "bitmap": "CIA402NetPos",

   124

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

   125

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

   126

            "method": "_getCIA402NetworkPosition",

   127

            "push": True,

   128

        },

   129

    ]

   130

   131

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

   132

#    class code

   133

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

   134

   135

    def __init__(self):

   136

        _EthercatSlaveCTN.__init__(self)

   137

   138

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

   139

        self.CommonMethod = _CommonSlave(self)

   140

   141

    def GetIconName(self):

   142

        return "CIA402Slave"

   143

   144

    def SetParamsAttribute(self, path, value):

   145

        if path == "CIA402SlaveParams.Type":

   146

            path = "SlaveParams.Type"

   147

        elif path == "CIA402SlaveParams.Alias":

   148

            path = "SlaveParams.Alias"

   149

        return _EthercatSlaveCTN.SetParamsAttribute(self, path, value)

   150

   151

    def GetVariableLocationTree(self):

   152

        axis_name = self.CTNName()

   153

        current_location = self.GetCurrentLocation()

   154

        children = [

   155

            {

   156

                "name": name_frmt % (axis_name),

   157

                "type": LOCATION_VAR_INPUT,

   158

                "size": "W",

   159

                "IEC_type": iec_type,

   160

                "var_name": var_name_frmt % axis_name,

   161

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

   162

                "description": "",

   163

                "children": []

   164

            }

   165

            for name_frmt, iec_type, var_name_frmt, location_frmt in [

   166

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

   167

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

   168

            ]

   169

        ]

   170

        children.extend(self.CTNParent.GetDeviceLocationTree(

   171

                            self.GetSlavePos(), current_location, axis_name))

   172

        return {

   173

            "name": axis_name,

   174

            "type": LOCATION_CONFNODE,

   175

            "location": self.GetFullIEC_Channel(),

   176

            "children": children,

   177

        }

   178

   179

    def CTNGlobalInstances(self):

   180

        current_location = self.GetCurrentLocation()

   181

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

   182

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

   183

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

   184

                for block_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES]

   185

   186

    def StartDragNDrop(self, data):

   187

        data_obj = wx.TextDataObject(str(data))

   188

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

   189

        dragSource.SetData(data_obj)

   190

        dragSource.DoDragDrop()

   191

   192

    def _getCIA402NetworkPosition(self):

   193

        self.StartDragNDrop(

   194

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

   195

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

   196

   197

    def _getCIA402AxisRef(self):

   198

        self.StartDragNDrop(

   199

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

   200

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

   201

   202

    def CTNGenerate_C(self, buildpath, locations):

   203

        current_location = self.GetCurrentLocation()

   204

   205

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

   206

        slave_pos = self.GetSlavePos()

   207

        MCL_headers = Headers

   208

   209

        # Open CIA402 node code template file

   210

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

   211

                                               "plc_cia402node.c")

   212

        plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   213

        plc_cia402node_code = plc_cia402node_file.read()

   214

        plc_cia402node_file.close()

   215

   216

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

   217

        fieldbus_interface_declaration = []

   218

        fieldbus_interface_definition = []

   219

        init_axis_params = []

   220

        extra_variables_retrieve = []

   221

        extra_variables_publish = []

   222

        extern_located_variables_declaration = []

   223

        entry_variables = []

   224

        init_entry_variables = []

   225

   226

        # Fieldbus interface code sections

   227

        for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES:

   228

            blocktype = blocktype_infos["blocktype"]

   229

            ucase_blocktype = blocktype.upper()

   230

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

   231

   232

            extract_inputs = "\n".join(["""\

   233

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

   234

                for (input_name, input_value) in [

   235

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

   236

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

   237

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

   238

                    for input in blocktype_infos["inputs"]]

   239

                ])

   240

   241

            return_outputs = "\n".join([

   242

                """\

   243

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

   244

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

   245

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

   246

                        output["name"].upper()

   247

                        for output in blocktype_infos["outputs"]]

   248

            ])

   249

   250

            fieldbus_interface_declaration.append("""

   251

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

   252

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

   253

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

   254

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

   255

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

   256

%(extract_inputs)s

   257

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

   258

%(return_outputs)s

   259

}""" % locals())

   260

   261

            fieldbus_interface_definition.append("""\

   262

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

   263

""" % locals())

   264

   265

        # Get a copy list of default variables to map

   266

        variables = NODE_VARIABLES[:]

   267

   268

        # Set AxisRef public struct members value

   269

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

   270

        for param in node_params["children"]:

   271

            param_name = param["name"]

   272

   273

            # Param is optional variables section enable flag

   274

            extra_node_variable_infos = EXTRA_NODE_VARIABLES_DICT.get(param_name)

   275

            if extra_node_variable_infos is not None:

   276

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

   277

   278

                if not param["value"]:

   279

                    continue

   280

   281

                # Optional variables section is enabled

   282

                for variable_infos in extra_node_variable_infos:

   283

                    var_name = variable_infos["description"][0]

   284

   285

                    # Add each variables defined in section description to the

   286

                    # list of variables to map

   287

                    variables.append(variable_infos["description"])

   288

   289

                    # Add code to publish or retrive variable

   290

                    for var_exchange_dir, str_list, default_template in [

   291

                         ("retrieve", extra_variables_retrieve,

   292

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

   293

                         ("publish", extra_variables_publish,

   294

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

   295

   296

                        template = variable_infos.get(var_exchange_dir,

   297

                                                      default_template)

   298

                        if template is not None:

   299

                            extra_variables_publish.append(template % locals())

   300

   301

            # Set AxisRef public struct member value if defined

   302

            if param["value"] is not None:

   303

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

   304

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

   305

                               else str(param["value"]))

   306

   307

                init_axis_params.append("""\

   308

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

   309

   310

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

   311

        # variables to add to network configuration

   312

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

   313

            var_size = self.GetSizeOfType(var_type)

   314

            var_name = """\

   315

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

   316

   317

            extern_located_variables_declaration.append(

   318

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

   319

            entry_variables.append(

   320

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

   321

            init_entry_variables.append(

   322

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

   323

   324

            self.CTNParent.FileGenerator.DeclareVariable(

   325

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

   326

   327

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

   328

        [fieldbus_interface_declaration, fieldbus_interface_definition,

   329

         init_axis_params, extra_variables_retrieve, extra_variables_publish,

   330

         extern_located_variables_declaration, entry_variables,

   331

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

   332

            fieldbus_interface_declaration, fieldbus_interface_definition,

   333

            init_axis_params, extra_variables_retrieve, extra_variables_publish,

   334

            extern_located_variables_declaration, entry_variables,

   335

            init_entry_variables])

   336

   337

        # Write generated content to CIA402 node file

   338

        Gen_CIA402Nodefile_path = os.path.join(buildpath,

   339

                                               "cia402node_%s.c" % location_str)

   340

        cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   341

        cia402nodefile.write(plc_cia402node_code % locals())

   342

        cia402nodefile.close()

   343

   344

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