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

    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

    # add jblee

   205

    """

   206

    def LoadPDOSelectData(self):

   207

        ReadData = []

   208

        files = os.listdir(self.CTNPath())

   209

        filepath = os.path.join(self.CTNPath(), "DataForPDO.txt")

   210

        if os.path.isfile(filepath):

   211

            PDODataRead = open(filepath, 'r')

   212

            ReadData = PDODataRead.readlines()

   213

            PDODataRead.close()

   214

   215

        if len(ReadData) > 1:

   216

            for data in ReadData[0].split() :

   217

                if data == "RxPDO":

   218

                    continue

   219

                self.SelectedRxPDOIndex.append(int(data, 0))

   220

   221

            for data in ReadData[1].split() :

   222

                if data == "TxPDO":

   223

                    continue

   224

                self.SelectedTxPDOIndex.append(int(data, 0))

   225

    """

   226

   227

    def LoadPDOSelectData(self):

   228

        RxPDOData = self.BaseParams.getRxPDO()

   229

        RxPDOs = []

   230

        if RxPDOData != "None":

   231

            RxPDOs = RxPDOData.split()

   232

        if RxPDOs :

   233

            for RxPDO in RxPDOs :

   234

                self.SelectedRxPDOIndex.append(int(RxPDO, 0))

   235

   236

        TxPDOData = self.BaseParams.getTxPDO()

   237

        TxPDOs = []

   238

        if TxPDOData != "None":

   239

            TxPDOs = TxPDOData.split()

   240

        if TxPDOs :

   241

            for TxPDO in TxPDOs :

   242

                self.SelectedTxPDOIndex.append(int(TxPDO, 0))

   243

   244

    def LoadDefaultPDOSet(self):

   245

        ReturnData = []

   246

        rx_pdo_entries = self.CommonMethod.GetRxPDOCategory()

   247

        if len(rx_pdo_entries):

   248

            for i in range(len(rx_pdo_entries)):

   249

                if rx_pdo_entries[i]['sm'] is not None:

   250

                    ReturnData.append(rx_pdo_entries[i]['pdo_index'])

   251

   252

        tx_pdo_entries = self.CommonMethod.GetTxPDOCategory()

   253

        if len(tx_pdo_entries):

   254

            for i in range(len(tx_pdo_entries)):

   255

                if tx_pdo_entries[i]['sm'] is not None:

   256

                    ReturnData.append(tx_pdo_entries[i]['pdo_index'])

   257

   258

        if ReturnData :

   259

            return ReturnData

   260

        else :

   261

            return [5632, 6656]

   262

   263

    def CTNGenerate_C(self, buildpath, locations):

   264

        current_location = self.GetCurrentLocation()

   265

   266

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

   267

   268

        # Open CIA402 node code template file

   269

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

   270

                                               "plc_cia402node.c")

   271

        plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   272

        plc_cia402node_code = plc_cia402node_file.read()

   273

        plc_cia402node_file.close()

   274

        # HSAHN 150726

   275

        # add "default_variables_retrieve": [], "default_variables_publish": [],

   276

		# As PDO mapping object, it will add auto-complete code.

   277

        # add "modeofop_homing_method", "modeofop_computation_mode" by jblee

   278

        str_completion = {

   279

            "slave_pos": self.GetSlavePos(),

   280

            "location": location_str,

   281

            "MCL_headers": Headers,

   282

            "extern_located_variables_declaration": [],

   283

            "fieldbus_interface_declaration": [],

   284

            "fieldbus_interface_definition": [],

   285

            "entry_variables": [],

   286

            "init_axis_params": [],

   287

            "init_entry_variables": [],

   288

            "default_variables_retrieve": [],

   289

            "default_variables_publish": [],

   290

            "extra_variables_retrieve": [],

   291

            "extra_variables_publish": [],

   292

            "modeofop_homing_method": [],

   293

            "modeofop_computation_mode": []

   294

        }

   295

   296

        for blocktype_infos in FIELDBUS_INTERFACE_GLOBAL_INSTANCES:

   297

            texts = {

   298

                "blocktype": blocktype_infos["blocktype"],

   299

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

   300

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

   301

            }

   302

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

   303

   304

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

   305

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

   306

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

   307

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

   308

                      for input in blocktype_infos["inputs"]]

   309

            input_texts = []

   310

            for input_infos in inputs:

   311

                input_infos.update(texts)

   312

                input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos)

   313

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

   314

   315

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

   316

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

   317

            output_texts = []

   318

            for output_infos in outputs:

   319

                output_infos.update(texts)

   320

                output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos)

   321

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

   322

   323

            str_completion["fieldbus_interface_declaration"].append(

   324

                    BLOCK_FUNCTION_TEMPLATE % texts)

   325

   326

            str_completion["fieldbus_interface_definition"].append(

   327

                    BLOCK_FUNTION_DEFINITION_TEMPLATE % texts)

   328

   329

        variables = NODE_VARIABLES[:]

   330

   331

#HSAHN

   332

#2015. 7. 24 PDO Variable

   333

        #if PDO is not selected, use 1st PDO set

   334

        self.LoadPDOSelectData()

   335

        if not self.SelectedRxPDOIndex and not self.SelectedTxPDOIndex :

   336

            self.SelectedPDOIndex = self.LoadDefaultPDOSet()

   337

        else :

   338

            self.SelectedPDOIndex = self.SelectedRxPDOIndex + self.SelectedTxPDOIndex

   339

   340

        add_idx = []

   341

        for i in range(len(ADD_NODE_VARIABLES)):

   342

            add_idx.append(ADD_NODE_VARIABLES[i]['index'])

   343

   344

        self.CommonMethod.RequestPDOInfo()

   345

        pdo_info = self.CommonMethod.GetRxPDOCategory() + self.CommonMethod.GetTxPDOCategory()

   346

        pdo_entry = self.CommonMethod.GetRxPDOInfo() + self.CommonMethod.GetTxPDOInfo()

   347

        list_index = 0

   348

        ModeOfOpFlag = False

   349

        ModeOfOpDisplayFlag = False

   350

        for i in range(len(pdo_info)):

   351

            #if pdo_index is in the SelectedPDOIndex: put the PDO mapping information intto the "used" object

   352

            if pdo_info[i]['pdo_index'] in self.SelectedPDOIndex:

   353

                used = pdo_entry[list_index:list_index + pdo_info[i]['number_of_entry']]

   354

                for used_data in used:

   355

                    # 24672 -> 0x6060, Mode of Operation

   356

                    if used_data['entry_index'] == 24672:

   357

                        ModeOfOpFlag = True

   358

                    # 24673 -> 0x6061, Mode of Operation Display

   359

                    elif used_data["entry_index"] == 24673:

   360

                        ModeOfOpDisplayFlag = True

   361

   362

                    if used_data['entry_index'] in add_idx:

   363

                        idx = add_idx.index(used_data['entry_index'])

   364

                        adder = list([ADD_NODE_VARIABLES[idx]['name'], ADD_NODE_VARIABLES[idx]['index'], \

   365

                                     ADD_NODE_VARIABLES[idx]['sub-index'], ADD_NODE_VARIABLES[idx]['type'], \

   366

                                     ADD_NODE_VARIABLES[idx]['direction']])

   367

                        variables.append(adder)

   368

                        if ADD_NODE_VARIABLES[idx]['direction'] == "Q":                           

   369

                            parsed_string = ADD_NODE_VARIABLES[idx]['name'].replace("Target", "")

   370

                            # add jblee

   371

                            check_q_data = "    *(AxsPub.Target%s) = AxsPub.axis->Raw%sSetPoint;" %(parsed_string, parsed_string)

   372

                            if check_q_data not in str_completion["default_variables_publish"]:

   373

                                str_completion["default_variables_publish"].append(check_q_data)

   374

                        elif ADD_NODE_VARIABLES[idx]['direction'] == "I":

   375

                            parsed_string = ADD_NODE_VARIABLES[idx]['name'].replace("Actual", "")

   376

                            # add jblee

   377

                            check_i_data = "    AxsPub.axis->ActualRaw%s = *(AxsPub.Actual%s);" %(parsed_string, parsed_string)

   378

                            if check_i_data not in str_completion["default_variables_retrieve"]:

   379

                                str_completion["default_variables_retrieve"].append(check_i_data)

   380

            list_index += pdo_info[i]['number_of_entry']

   381

#HSAHN END

   382

   383

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

   384

        for param in params["children"]:

   385

            if param["name"] in EXTRA_NODE_VARIABLES_DICT:

   386

                if param["value"]:

   387

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

   388

                    for variable_infos in extra_variables:

   389

                        var_infos = {

   390

                            "location": location_str,

   391

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

   392

                        }

   393

                        variables.append(variable_infos["description"])

   394

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

   395

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

   396

   397

                        if retrieve_template is not None:

   398

                            str_completion["extra_variables_retrieve"].append(

   399

                                retrieve_template % var_infos)

   400

                        if publish_template is not None:

   401

                            str_completion["extra_variables_publish"].append(

   402

                                publish_template % var_infos)

   403

   404

            #elif param["value"] is not None:

   405

            if param["value"] is not None:

   406

                param_infos = {

   407

                    "location": location_str,

   408

                    "param_name": param["name"],

   409

                }

   410

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

   411

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

   412

                    param_infos["param_name"] = param["name"].replace("Enable", "") + "Enabled"

   413

                    if param["value"] == False:

   414

                        continue

   415

                else:

   416

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

   417

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

   418

                str_completion["init_axis_params"].append(

   419

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

   420

   421

        check_variable = []

   422

        for variable in variables:

   423

            # add jblee

   424

            if variable in check_variable:

   425

                continue

   426

   427

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

   428

            var_infos["location"] = location_str

   429

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

   430

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

   431

   432

            # add jblee

   433

            if var_infos["index"] in [24672] and ModeOfOpFlag:

   434

                str_completion["modeofop_homing_method"].append(MODEOFOP_HOMING_METHOD_TEMPLATE)

   435

                str_completion["modeofop_computation_mode"].append(MODEOFOP_COMPUTATION_MODE_TEMPLATE)

   436

   437

            # add jblee

   438

            if var_infos["index"] in [24672, 24673] and (not ModeOfOpFlag or not ModeOfOpDisplayFlag):

   439

                continue

   440

   441

            str_completion["extern_located_variables_declaration"].append(

   442

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

   443

            str_completion["entry_variables"].append(

   444

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

   445

            str_completion["init_entry_variables"].append(

   446

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

   447

   448

            self.CTNParent.FileGenerator.DeclareVariable(

   449

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

   450

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

   451

   452

            # add jblee

   453

            check_variable.append(variable)

   454

   455

        for element in ["extern_located_variables_declaration", 

   456

                        "fieldbus_interface_declaration",

   457

                        "fieldbus_interface_definition",

   458

                        "entry_variables", 

   459

                        "init_axis_params", 

   460

                        "init_entry_variables",

   461

                        "default_variables_retrieve",

   462

                        "default_variables_publish",

   463

                        "extra_variables_retrieve",

   464

                        "extra_variables_publish",

   465

                        "modeofop_homing_method",

   466

                        "modeofop_computation_mode"]:

   467

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

   468

   469

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

   470

        cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   471

        cia402nodefile.write(plc_cia402node_code % str_completion)

   472

        cia402nodefile.close()

   473

   474

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