1

import os, shutil

     2

import cPickle

     3

from xml.dom import minidom

     4

     5

import wx

     6

import csv

     7

     8

from xmlclass import *

     9

from POULibrary import POULibrary

    10

from ConfigTreeNode import ConfigTreeNode

    11

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

    12

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

    13

    14

try:

    15

    from MotionLibrary import Headers, AxisXSD

    16

    HAS_MCL = True

    17

except:

    18

    HAS_MCL = False

    19

    20

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

    21

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

    22

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

    23

    24

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

    25

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

    26

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

    27

    28

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

    29

    30

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

    31

#         Remote Exec Etherlab Commands

    32

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

    33

    34

SCAN_COMMAND = """

    35

import commands

    36

result = commands.getoutput("ethercat slaves")

    37

slaves = []

    38

for slave_line in result.splitlines():

    39

    chunks = slave_line.split()

    40

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

    41

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

    42

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

    43

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

    44

             "alias": int(alias),

    45

             "position": int(position),

    46

             "name": name}

    47

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

    48

    for details_line in details.splitlines():

    49

        details_line = details_line.strip()

    50

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

    51

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

    52

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

    53

            if details_line.startswith(header):

    54

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

    55

                break

    56

    slaves.append(slave)

    57

returnVal = slaves

    58

"""

    59

    60

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

    61

#      Etherlab Specific Blocks Library

    62

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

    63

    64

def GetLocalPath(filename):

    65

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

    66

    67

class EtherlabLibrary(POULibrary):

    68

    def GetLibraryPath(self):

    69

        return GetLocalPath("pous.xml")

    70

    71

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

    72

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

    73

        etherlab_ext_code = etherlab_ext_file.read()

    74

        etherlab_ext_file.close()

    75

    76

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

    77

        ethelabfile = open(Gen_etherlabfile_path,'w')

    78

        ethelabfile.write(etherlab_ext_code)

    79

        ethelabfile.close()

    80

    81

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

    82

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

    83

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

    84

    85

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

    86

#                    Ethercat Node

    87

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

    88

    89

class _EthercatSlaveCTN:

    90

    91

    NODE_PROFILE = None

    92

    EditorType = NodeEditor

    93

    94

    def GetIconName(self):

    95

        return "Slave"

    96

    97

    def ExtractHexDecValue(self, value):

    98

        return ExtractHexDecValue(value)

    99

   100

    def GetSizeOfType(self, type):

   101

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

   102

   103

    def GetSlavePos(self):

   104

        return self.BaseParams.getIEC_Channel()

   105

   106

    def GetParamsAttributes(self, path = None):

   107

        if path:

   108

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

   109

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

   110

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

   111

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

   112

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

   113

        else:

   114

            params = []

   115

            if self.CTNParams:

   116

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

   117

            else:

   118

                params.append({

   119

                    'use': 'required', 

   120

                    'type': 'element', 

   121

                    'name': 'SlaveParams', 

   122

                    'value': None, 

   123

                    'children': []

   124

                })

   125

   126

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

   127

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

   128

                   {'use': 'optional', 

   129

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

   130

                    'name': 'Type', 

   131

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

   132

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

   133

                   {'use': 'optional', 

   134

                    'type': 'unsignedLong', 

   135

                    'name': 'Alias', 

   136

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

   137

            return params

   138

   139

    def SetParamsAttribute(self, path, value):

   140

        position = self.BaseParams.getIEC_Channel()

   141

   142

        if path == "SlaveParams.Type":

   143

            self.CTNParent.SetSlaveType(position, value)

   144

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

   145

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

   146

            if self._View is not None:

   147

                wx.CallAfter(self._View.RefreshSlaveInfos)

   148

            return value, True

   149

        elif path == "SlaveParams.Alias":

   150

            self.CTNParent.SetSlaveAlias(position, value)

   151

            return value, True

   152

   153

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

   154

   155

        # Filter IEC_Channel, Slave_Type and Alias that have specific behavior

   156

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

   157

            self.CTNParent.SetSlavePosition(position, value)

   158

   159

        return value, refresh

   160

   161

    def GetSlaveInfos(self):

   162

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

   163

   164

    def GetSlaveVariables(self, limits):

   165

        return self.CTNParent.GetSlaveVariables(self.GetSlavePos(), limits)

   166

   167

    def GetVariableLocationTree(self):

   168

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

   169

                 "type": LOCATION_CONFNODE,

   170

                 "location": self.GetFullIEC_Channel(),

   171

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

   172

        }

   173

   174

    def CTNGenerate_C(self, buildpath, locations):

   175

        """

   176

        Generate C code

   177

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

   178

        @param locations: List of complete variables locations \

   179

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

   180

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

   181

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

   182

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

   183

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

   184

            }, ...]

   185

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

   186

        """

   187

        return [],"",False

   188

   189

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

   190

#                 Ethercat CIA402 Node

   191

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

   192

   193

if HAS_MCL:

   194

   195

    NODE_VARIABLES = [

   196

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

   197

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

   198

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

   199

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

   200

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

   201

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

   202

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

   203

    ]

   204

   205

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

   206

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

   207

   208

    EXTRA_NODE_VARIABLES = [

   209

        ("ErrorCode", [

   210

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

   211

             "publish": None}

   212

            ]),

   213

        ("DigitalInputs", [

   214

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

   215

             "publish": None}

   216

            ]),

   217

        ("DigitalOutputs", [

   218

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

   219

             "retrieve": None}

   220

            ])

   221

    ]

   222

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

   223

   224

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

   225

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

   226

   227

    BLOCK_FUNCTION_TEMPLATE = """

   228

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

   229

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

   230

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

   231

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

   232

%(extract_inputs)s

   233

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

   234

%(return_outputs)s

   235

}

   236

"""

   237

   238

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

   239

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

   240

   241

    GLOBAL_INSTANCES = [

   242

        {"blocktype": "GetTorqueLimit", 

   243

         "inputs": [],

   244

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

   245

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

   246

        {"blocktype": "SetTorqueLimit", 

   247

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

   248

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

   249

         "outputs": []},

   250

    ]

   251

   252

    class _EthercatCIA402SlaveCTN(_EthercatSlaveCTN):

   253

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

   254

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

   255

          <xsd:element name="CIA402SlaveParams">

   256

            <xsd:complexType>

   257

              %s

   258

            </xsd:complexType>

   259

          </xsd:element>

   260

        </xsd:schema>

   261

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

   262

                          for category, variables in EXTRA_NODE_VARIABLES]) + AxisXSD)

   263

   264

        NODE_PROFILE = 402

   265

        EditorType = CIA402NodeEditor

   266

   267

        ConfNodeMethods = [

   268

            {"bitmap" : "CIA402AxisRef",

   269

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

   270

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

   271

             "method" : "_getCIA402AxisRef",

   272

             "push": True},

   273

        ]

   274

   275

        def GetIconName(self):

   276

            return "CIA402Slave"

   277

   278

        def SetParamsAttribute(self, path, value):

   279

            if path == "CIA402SlaveParams.Type":

   280

                path = "SlaveParams.Type"

   281

            elif path == "CIA402SlaveParams.Alias":

   282

                path = "SlaveParams.Alias"

   283

            return _EthercatSlaveCTN.SetParamsAttribute(self, path, value)

   284

   285

        def GetVariableLocationTree(self):

   286

            axis_name = self.CTNName()

   287

            current_location = self.GetCurrentLocation()

   288

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

   289

                         "type": LOCATION_VAR_INPUT,

   290

                         "size": "W",

   291

                         "IEC_type": "AXIS_REF",

   292

                         "var_name": axis_name,

   293

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

   294

                         "description": "",

   295

                         "children": []}]

   296

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

   297

            return  {"name": axis_name,

   298

                     "type": LOCATION_CONFNODE,

   299

                     "location": self.GetFullIEC_Channel(),

   300

                     "children": children,

   301

            }

   302

   303

        def CTNGlobalInstances(self):

   304

            current_location = self.GetCurrentLocation()

   305

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

   306

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

   307

   308

        def _getCIA402AxisRef(self):

   309

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

   310

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

   311

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

   312

            dragSource.SetData(data)

   313

            dragSource.DoDragDrop()

   314

   315

        def CTNGenerate_C(self, buildpath, locations):

   316

            """

   317

            Generate C code

   318

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

   319

            @param locations: List of complete variables locations \

   320

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

   321

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

   322

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

   323

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

   324

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

   325

                }, ...]

   326

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

   327

            """

   328

            current_location = self.GetCurrentLocation()

   329

   330

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

   331

   332

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

   333

            plc_cia402node_file = open(plc_cia402node_filepath, 'r')

   334

            plc_cia402node_code = plc_cia402node_file.read()

   335

            plc_cia402node_file.close()

   336

   337

            str_completion = {

   338

                "slave_pos": self.GetSlavePos(),

   339

                "location": location_str,

   340

                "MCL_headers": Headers,

   341

                "extern_located_variables_declaration": [],

   342

                "fieldbus_interface_declaration": [],

   343

                "fieldbus_interface_definition": [],

   344

                "entry_variables": [],

   345

                "init_axis_params": [],

   346

                "init_entry_variables": [],

   347

                "extra_variables_retrieve": [],

   348

                "extra_variables_publish": []

   349

            }

   350

   351

            for blocktype_infos in GLOBAL_INSTANCES:

   352

                texts = {

   353

                    "blocktype": blocktype_infos["blocktype"],

   354

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

   355

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

   356

                }

   357

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

   358

   359

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

   360

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

   361

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

   362

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

   363

                          for input in blocktype_infos["inputs"]]

   364

                input_texts = []

   365

                for input_infos in inputs:

   366

                    input_infos.update(texts)

   367

                    input_texts.append(BLOCK_INPUT_TEMPLATE % input_infos)

   368

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

   369

   370

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

   371

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

   372

                output_texts = []

   373

                for output_infos in outputs:

   374

                    output_infos.update(texts)

   375

                    output_texts.append(BLOCK_OUTPUT_TEMPLATE % output_infos)

   376

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

   377

   378

                str_completion["fieldbus_interface_declaration"].append(

   379

                        BLOCK_FUNCTION_TEMPLATE % texts)

   380

   381

                str_completion["fieldbus_interface_definition"].append(

   382

                        BLOCK_FUNTION_DEFINITION_TEMPLATE % texts)

   383

   384

            variables = NODE_VARIABLES[:]

   385

   386

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

   387

            for param in params["children"]:

   388

                if param["name"] in EXTRA_NODE_VARIABLES_DICT:

   389

                    if param["value"]:

   390

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

   391

                        for variable_infos in extra_variables:

   392

                            var_infos = {

   393

                                "location": location_str,

   394

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

   395

                            }

   396

                            variables.append(variable_infos["description"])

   397

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

   398

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

   399

   400

                            if retrieve_template is not None:

   401

                                str_completion["extra_variables_retrieve"].append(

   402

                                    retrieve_template % var_infos)

   403

                            if publish_template is not None:

   404

                                str_completion["extra_variables_publish"].append(

   405

                                    publish_template % var_infos)

   406

                elif param["value"] is not None:

   407

                    param_infos = {

   408

                        "location": location_str,

   409

                        "param_name": param["name"],

   410

                    }

   411

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

   412

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

   413

                    else:

   414

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

   415

                    str_completion["init_axis_params"].append(

   416

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

   417

   418

            for variable in variables:

   419

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

   420

                var_infos["location"] = location_str

   421

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

   422

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

   423

   424

                str_completion["extern_located_variables_declaration"].append(

   425

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

   426

                str_completion["entry_variables"].append(

   427

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

   428

                str_completion["init_entry_variables"].append(

   429

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

   430

   431

                self.CTNParent.FileGenerator.DeclareVariable(

   432

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

   433

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

   434

   435

            for element in ["extern_located_variables_declaration", 

   436

                            "fieldbus_interface_declaration",

   437

                            "fieldbus_interface_definition",

   438

                            "entry_variables", 

   439

                            "init_axis_params", 

   440

                            "init_entry_variables",

   441

                            "extra_variables_retrieve",

   442

                            "extra_variables_publish"]:

   443

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

   444

   445

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

   446

            cia402nodefile = open(Gen_CIA402Nodefile_path, 'w')

   447

            cia402nodefile.write(plc_cia402node_code % str_completion)

   448

            cia402nodefile.close()

   449

   450

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

   451

   452

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

   453

#                 Ethercat MASTER

   454

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

   455

   456

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

   457

   458

def ExtractHexDecValue(value):

   459

    try:

   460

        return int(value)

   461

    except:

   462

        pass

   463

    try:

   464

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

   465

    except:

   466

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

   467

   468

def GenerateHexDecValue(value, base=10):

   469

    if base == 10:

   470

        return str(value)

   471

    elif base == 16:

   472

        return "#x%.8x" % value

   473

    else:

   474

        raise ValueError, "Not supported base"

   475

   476

def sort_commands(x, y):

   477

    if x["Index"] == y["Index"]:

   478

        return cmp(x["Subindex"], y["Subindex"])

   479

    return cmp(x["Index"], y["Index"])

   480

   481

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

   482

if cls:

   483

   484

    def getType(self):

   485

        slave_info = self.getInfo()

   486

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

   487

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

   488

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

   489

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

   490