1

import os, shutil

     2

import cPickle

     3

from xml.dom import minidom

     4

     5

import wx

     6

     7

from xmlclass import *

     8

from POULibrary import POULibrary

     9

from ConfigTreeNode import ConfigTreeNode

    10

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

    11

from ConfigEditor import NodeEditor, CIA402NodeEditor, ETHERCAT_VENDOR, ETHERCAT_GROUP, ETHERCAT_DEVICE

    12

    13

try:

    14

    from MotionLibrary import Headers, AxisXSD

    15

    HAS_MCL = True

    16

except:

    17

    HAS_MCL = False

    18

    19

TYPECONVERSION = {"BOOL" : "X", "SINT" : "B", "INT" : "W", "DINT" : "D", "LINT" : "L",

    20

    "USINT" : "B", "UINT" : "W", "UDINT" : "D", "ULINT" : "L", 

    21

    "BYTE" : "B", "WORD" : "W", "DWORD" : "D", "LWORD" : "L"}

    22

    23

DATATYPECONVERSION = {"BOOL" : "BIT", "SINT" : "S8", "INT" : "S16", "DINT" : "S32", "LINT" : "S64",

    24

    "USINT" : "U8", "UINT" : "U16", "UDINT" : "U32", "ULINT" : "U64", 

    25

    "BYTE" : "U8", "WORD" : "U16", "DWORD" : "U32", "LWORD" : "U64"}

    26

    27

VARCLASSCONVERSION = {"T": LOCATION_VAR_INPUT, "R": LOCATION_VAR_OUTPUT, "RT": LOCATION_VAR_MEMORY}

    28

    29

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

    30

#         Remote Exec Etherlab Commands

    31

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

    32

    33

SCAN_COMMAND = """

    34

import commands

    35

result = commands.getoutput("ethercat slaves")

    36

slaves = []

    37

for slave_line in result.splitlines():

    38

    chunks = slave_line.split()

    39

    idx, pos, state, flag = chunks[:4]

    40

    name = " ".join(chunks[4:])

    41

    alias, position = pos.split(":")

    42

    slave = {"idx": int(idx),

    43

             "alias": int(alias),

    44

             "position": int(position),

    45

             "name": name}

    46

    details = commands.getoutput("ethercat slaves -p %d -v" % slave["idx"])

    47

    for details_line in details.splitlines():

    48

        details_line = details_line.strip()

    49

        for header, param in [("Vendor Id:", "vendor_id"),

    50

                              ("Product code:", "product_code"),

    51

                              ("Revision number:", "revision_number")]:

    52

            if details_line.startswith(header):

    53

                slave[param] = details_line.split()[-1]

    54

                break

    55

    slaves.append(slave)

    56

returnVal = slaves

    57

"""

    58

    59

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

    60

#      Etherlab Specific Blocks Library

    61

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

    62

    63

def GetLocalPath(filename):

    64

    return os.path.join(os.path.split(__file__)[0], filename)

    65

    66

class EtherlabLibrary(POULibrary):

    67

    def GetLibraryPath(self):

    68

        return GetLocalPath("pous.xml")

    69

    70

    def Generate_C(self, buildpath, varlist, IECCFLAGS):

    71

        etherlab_ext_file = open(GetLocalPath("etherlab_ext.c"), 'r')

    72

        etherlab_ext_code = etherlab_ext_file.read()

    73

        etherlab_ext_file.close()

    74

    75

        Gen_etherlabfile_path = os.path.join(buildpath, "etherlab_ext.c")

    76

        ethelabfile = open(Gen_etherlabfile_path,'w')

    77

        ethelabfile.write(etherlab_ext_code)

    78

        ethelabfile.close()

    79

    80

        runtimefile_path = os.path.join(os.path.split(__file__)[0], "runtime_etherlab.py")

    81

        return ((["etherlab_ext"], [(Gen_etherlabfile_path, IECCFLAGS)], True), "", 

    82

                ("runtime_etherlab.py", file(GetLocalPath("runtime_etherlab.py"))))

    83

    84

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

    85

#                    Ethercat Node

    86

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

    87

    88

class _EthercatSlaveCTN:

    89

    90

    NODE_PROFILE = None

    91

    EditorType = NodeEditor

    92

    93

    def GetIconName(self):

    94

        return "Slave"

    95

    96

    def ExtractHexDecValue(self, value):

    97

        return ExtractHexDecValue(value)

    98

    99

    def GetSizeOfType(self, type):

   100

        return TYPECONVERSION.get(self.GetCTRoot().GetBaseType(type), None)

   101

   102

    def GetSlavePos(self):

   103

        return self.BaseParams.getIEC_Channel()

   104

   105

    def GetParamsAttributes(self, path = None):

   106

        if path:

   107

            parts = path.split(".", 1)

   108

            if self.MandatoryParams and parts[0] == self.MandatoryParams[0]:

   109

                return self.MandatoryParams[1].getElementInfos(parts[0], parts[1])

   110

            elif self.CTNParams and parts[0] == self.CTNParams[0]:

   111

                return self.CTNParams[1].getElementInfos(parts[0], parts[1])

   112

        else:

   113

            params = []

   114

            if self.CTNParams:

   115

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

   116

            else:

   117

                params.append({

   118

                    'use': 'required', 

   119

                    'type': 'element', 

   120

                    'name': 'SlaveParams', 

   121

                    'value': None, 

   122

                    'children': []

   123

                })

   124

   125

            slave_type = self.CTNParent.GetSlaveType(self.GetSlavePos())

   126

            params[0]['children'].insert(0,

   127

                   {'use': 'optional', 

   128

                    'type': self.CTNParent.GetSlaveTypesLibrary(self.NODE_PROFILE), 

   129

                    'name': 'Type', 

   130

                    'value': (slave_type["device_type"], slave_type)}) 

   131

            params[0]['children'].insert(1,

   132

                   {'use': 'optional', 

   133

                    'type': 'unsignedLong', 

   134

                    'name': 'Alias', 

   135

                    'value': self.CTNParent.GetSlaveAlias(self.GetSlavePos())})

   136

            return params

   137

   138

    def SetParamsAttribute(self, path, value):

   139

        position = self.BaseParams.getIEC_Channel()

   140

   141

        if path == "SlaveParams.Type":

   142

            self.CTNParent.SetSlaveType(position, value)

   143

            slave_type = self.CTNParent.GetSlaveType(self.GetSlavePos())

   144

            value = (slave_type["device_type"], slave_type)

   145

            if self._View is not None:

   146

                wx.CallAfter(self._View.RefreshSlaveInfos)

   147

            return value, True

   148

        elif path == "SlaveParams.Alias":

   149

            self.CTNParent.SetSlaveAlias(position, value)

   150

            return value, True

   151

   152

        value, refresh = ConfigTreeNode.SetParamsAttribute(self, path, value)

   153

   154

        # Filter IEC_Channel, Slave_Type and Alias that have specific behavior

   155

        if path == "BaseParams.IEC_Channel" and value != position:

   156

            self.CTNParent.SetSlavePosition(position, value)

   157

   158

        return value, refresh

   159

   160

    def GetSlaveInfos(self):

   161

        return self.CTNParent.GetSlaveInfos(self.GetSlavePos())

   162

   163

    def GetVariableLocationTree(self):

   164

        return  {"name": self.BaseParams.getName(),

   165

                 "type": LOCATION_CONFNODE,

   166

                 "location": self.GetFullIEC_Channel(),

   167

                 "children": self.CTNParent.GetDeviceLocationTree(self.GetSlavePos(), self.GetCurrentLocation(), self.BaseParams.getName())

   168

        }

   169

   170

    def CTNGenerate_C(self, buildpath, locations):

   171

        """

   172

        Generate C code

   173

        @param current_location: Tupple containing confnode IEC location : %I0.0.4.5 => (0,0,4,5)

   174

        @param locations: List of complete variables locations \

   175

            [{"IEC_TYPE" : the IEC type (i.e. "INT", "STRING", ...)

   176

            "NAME" : name of the variable (generally "__IW0_1_2" style)

   177

            "DIR" : direction "Q","I" or "M"

   178

            "SIZE" : size "X", "B", "W", "D", "L"

   179

            "LOC" : tuple of interger for IEC location (0,1,2,...)

   180

            }, ...]

   181

        @return: [(C_file_name, CFLAGS),...] , LDFLAGS_TO_APPEND

   182

        """

   183

        return [],"",False

   184

   185

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

   186

#                 Ethercat CIA402 Node

   187

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

   188

   189

if HAS_MCL:

   190

   191

    NODE_VARIABLES = [

   192

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

   193

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

   194

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

   195

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

   196

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

   197

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

   198

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

   199

    ]

   200

   201

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

   202

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

   203

   204

    EXTRA_NODE_VARIABLES = [

   209

        ("DigitalInputs", [

   210

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

   211

             "publish": None}

   216

            ])

   217

    ]

   218

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

   219

   220

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

   221

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

   222

   223

    BLOCK_FUNCTION_TEMPLATE = """

   224

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

   225

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

   226

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

   227

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

   228

%(extract_inputs)s

   229

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

   230

%(return_outputs)s

   231

}

   232

"""

   233

   234

    BLOCK_FUNTION_DEFINITION_TEMPLATE = """    if (!__CIA402Node_%(location)s.axis->__mcl_func_MC_%(blocktype)s)

   235

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

   236

   237

    GLOBAL_INSTANCES = [

   238

        {"blocktype": "GetTorqueLimit", 

   239

         "inputs": [],

   240

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

   241

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

   242

        {"blocktype": "SetTorqueLimit", 

   243

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

   244

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

   245

         "outputs": []},

   246

    ]

   247

   248

    class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):

   249

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

   250

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

   251

          <xsd:element name="CIA402SlaveParams">

   252

            <xsd:complexType>

   253

              %s

   254

            </xsd:complexType>

   255

          </xsd:element>

   256

        </xsd:schema>

   257

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

   258

                          for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)

   259

   260

        NODE_PROFILE = 402

   261

        EditorType = CIA402NodeEditor

   262

   263

        ConfNodeMethods = [

   264

            {"bitmap" : "CIA402AxisRef",

   265

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

   266

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

   267

             "method" : "_getCIA402AxisRef",

   268

             "push": True},

   269

        ]

   270

   271

        def GetIconName(self):

   272

            return "CIA402Slave"

   273

   274

        def SetParamsAttribute(self, path, value):

   275

            if path == "CIA402SlaveParams.Type":

   276

                path = "SlaveParams.Type"

   277

            elif path == "CIA402SlaveParams.Alias":

   278

                path = "SlaveParams.Alias"

   279

            return _EthercatSlaveCTN.SetParamsAttribute(self, path, value)

   280

   281

        def GetVariableLocationTree(self):

   282

            axis_name = self.CTNName()

   283

            current_location = self.GetCurrentLocation()

   284

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

   285

                         "type": LOCATION_VAR_INPUT,

   286

                         "size": "W",

   287

                         "IEC_type": "AXIS_REF",

   288

                         "var_name": axis_name,

   289

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

   290

                         "description": "",

   291

                         "children": []}]

   292

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

   293

            return  {"name": axis_name,

   294

                     "type": LOCATION_CONFNODE,

   295

                     "location": self.GetFullIEC_Channel(),

   296

                     "children": children,

   297

            }

   298

   299

        def CTNGlobalInstances(self):

   300

            current_location = self.GetCurrentLocation()

   301

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

   302

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

   303

   304

        def _getCIA402AxisRef(self):

   305

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

   306

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

   307

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

   308

            dragSource.SetData(data)

   309

            dragSource.DoDragDrop()

   310

   311

        def CTNGenerate_C(self, buildpath, locations):

   312

            """

   313

            Generate C code

   314

            @param current_location: Tupple containing confnode IEC location : %I0.0.4.5 => (0,0,4,5)

   315

            @param locations: List of complete variables locations \

   316

                [{"IEC_TYPE" : the IEC type (i.e. "INT", "STRING", ...)

   317

                "NAME" : name of the variable (generally "__IW0_1_2" style)

   318

                "DIR" : direction "Q","I" or "M"

   319

                "SIZE" : size "X", "B", "W", "D", "L"

   320

                "LOC" : tuple of interger for IEC location (0,1,2,...)

   321

                }, ...]

   322

            @return: [(C_file_name, CFLAGS),...] , LDFLAGS_TO_APPEND

   323

            """

   324

            current_location = self.GetCurrentLocation()

   325

   326

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

   327

   328

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

   329

            plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   330

            plc_cia402node_code = plc_cia402node_file.read()

   331

            plc_cia402node_file.close()

   332

   333

            str_completion = {

   334

                "slave_pos": self.GetSlavePos(),

   335

                "location": location_str,

   336

                "MCL_headers": Headers,

   337

                "extern_located_variables_declaration": [],

   338

                "fieldbus_interface_declaration": [],

   339

                "fieldbus_interface_definition": [],

   340

                "entry_variables": [],

   341

                "init_axis_params": [],

   342

                "init_entry_variables": [],

   343

                "extra_variables_retrieve": [],

   344

                "extra_variables_publish": []

   345

            }

   346

   347

            for blocktype_infos in GLOBAL_INSTANCES:

   348

                texts = {

   349

                    "blocktype": blocktype_infos["blocktype"],

   350

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

   351

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

   352

                }

   353

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

   354

   355

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

   356

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

   357

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

   358

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

   359

                          for input in blocktype_infos["inputs"]]

   360

                input_texts = []

   361

                for input_infos in inputs:

   362

                    input_infos.update(texts)

   363

                    input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos)

   364

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

   365

   366

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

   367

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

   368

                output_texts = []

   369

                for output_infos in outputs:

   370

                    output_infos.update(texts)

   371

                    output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos)

   372

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

   373

   374

                str_completion["fieldbus_interface_declaration"].append(

   375

                        BLOCK_FUNCTION_TEMPLATE % texts)

   376

   377

                str_completion["fieldbus_interface_definition"].append(

   378

                        BLOCK_FUNTION_DEFINITION_TEMPLATE % texts)

   379

   380

            variables = NODE_VARIABLES[:]

   381

   382

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

   383

            for param in params["children"]:

   384

                if param["name"] in EXTRA_NODE_VARIABLES_DICT:

   385

                    if param["value"]:

   386

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

   387

                        for variable_infos in extra_variables:

   388

                            var_infos = {

   389

                                "location": location_str,

   390

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

   391

                            }

   392

                            variables.append(variable_infos["description"])

   393

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

   394

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

   395

   396

                            if retrieve_template is not None:

   397

                                str_completion["extra_variables_retrieve"].append(

   398

                                    retrieve_template % var_infos)

   399

                            if publish_template is not None:

   400

                                str_completion["extra_variables_publish"].append(

   401

                                    publish_template % var_infos)

   402

                elif param["value"] is not None:

   403

                    param_infos = {

   404

                        "location": location_str,

   405

                        "param_name": param["name"],

   406

                    }

   407

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

   408

                        param_infos["param_value"] = {True: "true", False: "false"}[param["value"]]

   409

                    else:

   410

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

   411

                    str_completion["init_axis_params"].append(

   412

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

   413

   414

            for variable in variables:

   415

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

   416

                var_infos["location"] = location_str

   417

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

   418

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

   419

   420

                str_completion["extern_located_variables_declaration"].append(

   421

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

   422

                str_completion["entry_variables"].append(

   423

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

   424

                str_completion["init_entry_variables"].append(

   425

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

   426

   427

                self.CTNParent.FileGenerator.DeclareVariable(

   428

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

   429

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

   430

   431

            for element in ["extern_located_variables_declaration", 

   432

                            "fieldbus_interface_declaration",

   433

                            "fieldbus_interface_definition",

   434

                            "entry_variables", 

   435

                            "init_axis_params", 

   436

                            "init_entry_variables",

   437

                            "extra_variables_retrieve",

   438

                            "extra_variables_publish"]:

   439

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

   440

   441

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

   442

            cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   443

            cia402nodefile.write(plc_cia402node_code % str_completion)

   444

            cia402nodefile.close()

   445

   446

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

   447

   448

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

   449

#                 Ethercat MASTER

   450

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

   451

   452

EtherCATConfigClasses = GenerateClassesFromXSD(os.path.join(os.path.dirname(__file__), "EtherCATConfig.xsd")) 

   453

   454

def ExtractHexDecValue(value):

   455

    try:

   456

        return int(value)

   457

    except:

   458

        pass

   459

    try:

   460

        return int(value.replace("#", "0"), 16)

   461

    except:

   462

        raise ValueError, "Invalid value for HexDecValue \"%s\"" % value

   463

   464

def GenerateHexDecValue(value, base=10):

   465

    if base == 10:

   466

        return str(value)

   467

    elif base == 16:

   468

        return "#x%.8x" % value

   469

    else:

   470

        raise ValueError, "Not supported base"

   471

   472

cls = EtherCATConfigClasses.get("Config_Slave", None)

   473

if cls:

   474

   475

    def getType(self):

   476

        slave_info = self.getInfo()

   477

        return {"device_type": slave_info.getName(),

   478

                "vendor": GenerateHexDecValue(slave_info.getVendorId()),

   479

                "product_code": GenerateHexDecValue(slave_info.getProductCode(), 16),

   480

                "revision_number": GenerateHexDecValue(slave_info.getRevisionNo(), 16)}

   481

    setattr(cls, "getType", getType)

   482

   483

    def setType(self, type_infos):

   484

        slave_info = self.getInfo()

   485

        slave_info.setName(type_infos["device_type"])

   486

        slave_info.setVendorId(ExtractHexDecValue(type_infos["vendor"]))

   487

        slave_info.setProductCode(ExtractHexDecValue(type_infos["product_code"]))

   488

        slave_info.setRevisionNo(ExtractHexDecValue(type_infos["revision_number"]))

   489

    setattr(cls, "setType", setType)

   490