etherlab/plc_cia402node.c
author Edouard Tisserant
Mon, 15 Apr 2019 13:34:09 +0200
changeset 2594 f1e182818434
parent 2491 362039519454
child 2643 b98d9e08231f
permissions -rw-r--r--
Added PurgePLC() method to PLCObject, to be invoked when PLC Status is broken. It clears PLC shared object an extra files and force status to Empty. Added correponding Repair button to toolbar, visible only when PLC status is broken.
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
     1
/*
2165
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     2
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     3
Template C code used to produce target Ethercat C CIA402 code
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     4
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     5
Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     6
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     7
Distributed under the terms of the GNU Lesser General Public License as
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     8
published by the Free Software Foundation; either version 2 of the License, or
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
     9
(at your option) any later version.
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
    10
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
    11
See COPYING file for copyrights details.
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
    12
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
    13
*/
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    14
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    15
#include "ecrt.h"
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    16
2122
03cf08190ac4 Fixed header files inclusion in CIA402 node template file
Laurent Bessard
parents: 2118
diff changeset
    17
#include "beremiz.h"
03cf08190ac4 Fixed header files inclusion in CIA402 node template file
Laurent Bessard
parents: 2118
diff changeset
    18
#include "iec_types_all.h"
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    19
2094
72b74099b873 Fix bug in support for Etherlab specific fieldbus interface function blocks
Laurent Bessard
parents: 2092
diff changeset
    20
#include "accessor.h"
2092
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    21
#include "POUS.h"
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    22
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    23
/* From CiA402, page 27
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    24
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    25
        Table 30 - State coding
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    26
    Statusword      |      PDS FSA state
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    27
xxxx xxxx x0xx 0000 | Not ready to switch on
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    28
xxxx xxxx x1xx 0000 | Switch on disabled
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    29
xxxx xxxx x01x 0001 | Ready to switch on
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    30
xxxx xxxx x01x 0011 | Switched on
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    31
xxxx xxxx x01x 0111 | Operation enabled
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    32
xxxx xxxx x00x 0111 | Quick stop active
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    33
xxxx xxxx x0xx 1111 | Fault reaction active
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    34
xxxx xxxx x0xx 1000 | Fault
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    35
*/
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    36
#define FSAFromStatusWord(SW) (SW & 0x006f)
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    37
#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    38
#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    39
#define ReadyToSwitchOn     0b00100001
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    40
#define SwitchedOn          0b00100011
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    41
#define OperationEnabled    0b00100111
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    42
#define QuickStopActive     0b00000111
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    43
#define FaultReactionActive 0b00001111 FSA_sep 0b00101111
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    44
#define Fault               0b00001000 FSA_sep 0b00101000
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    45
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    46
// SatusWord bits :
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    47
#define SW_ReadyToSwitchOn     0x0001
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    48
#define SW_SwitchedOn          0x0002
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    49
#define SW_OperationEnabled    0x0004
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    50
#define SW_Fault               0x0008
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    51
#define SW_VoltageEnabled      0x0010
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    52
#define SW_QuickStop           0x0020
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    53
#define SW_SwitchOnDisabled    0x0040
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    54
#define SW_Warning             0x0080
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    55
#define SW_Remote              0x0200
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    56
#define SW_TargetReached       0x0400
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    57
#define SW_InternalLimitActive 0x0800
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    58
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    59
// ControlWord bits :
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    60
#define SwitchOn        0x0001
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    61
#define EnableVoltage   0x0002
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    62
#define QuickStop       0x0004
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    63
#define EnableOperation 0x0008
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    64
#define FaultReset      0x0080
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    65
#define Halt            0x0100
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    66
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
    67
2154
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
    68
IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s;
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
    69
IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s;
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
    70
IEC_INT beremiz__IW%(location_str)s_402;
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
    71
IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
    72
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
    73
%(MCL_headers)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    74
2059
b74955d211c7 fixed multiple cia402 instance symbol collision
Edouard Tisserant
parents: 2045
diff changeset
    75
static IEC_BOOL __FirstTick = 1;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    76
2491
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    77
typedef enum {
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    78
    mc_mode_none, // No motion mode
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    79
    mc_mode_csp,  // Continuous Synchronous Positionning mode
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    80
    mc_mode_csv,  // Continuous Synchronous Velocity mode
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    81
    mc_mode_cst,  // Continuous Synchronous Torque mode
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    82
} mc_axismotionmode_enum;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    83
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    84
typedef struct {
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    85
   IEC_BOOL Power;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    86
   IEC_BOOL CommunicationReady;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    87
   IEC_UINT NetworkPosition;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    88
   IEC_BOOL ReadyForPowerOn;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    89
   IEC_BOOL PowerFeedback;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    90
   IEC_DINT ActualRawPosition;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    91
   IEC_DINT ActualRawVelocity;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    92
   IEC_DINT ActualRawTorque;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    93
   IEC_DINT RawPositionSetPoint;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    94
   IEC_DINT RawVelocitySetPoint;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    95
   IEC_DINT RawTorqueSetPoint;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    96
   mc_axismotionmode_enum AxisMotionMode;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    97
   IEC_LREAL ActualVelocity;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    98
   IEC_LREAL ActualPosition;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    99
   IEC_LREAL ActualTorque;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   100
}axis_s;
362039519454 Added stub axis_s type in EtherLAB CiA402 support code, until Motion Contol Library comes back
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   101
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   102
typedef struct {
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   103
%(entry_variables)s
2077
14dfb786c53e Fix bug when modifying slave position
Laurent Bessard
parents: 2070
diff changeset
   104
    axis_s* axis;
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
   105
} __CIA402Node;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   106
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   107
#define AxsPub __CIA402Node_%(location_str)s
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   108
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   109
static __CIA402Node AxsPub;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   110
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   111
%(extern_located_variables_declaration)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   112
2092
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
   113
%(fieldbus_interface_declaration)s
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
   114
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   115
int __init_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   116
{
2118
c317b1aaf920 Fixed bug when using real axis and simulated axis at the same time (got the same index)
Laurent Bessard
parents: 2104
diff changeset
   117
    __FirstTick = 1;
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   118
%(init_entry_variables)s
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   119
	*(AxsPub.ModesOfOperation) = 0x08;
2079
49242019a9ca Fix C code Generator to use CoE section contained in the slave ESI file for defining Ethercat network configuration
Laurent Bessard
parents: 2077
diff changeset
   120
    return 0;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   121
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   122
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   123
void __cleanup_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   124
{
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   125
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   126
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   127
void __retrieve_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   128
{
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   129
	if (__FirstTick) {
2154
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
   130
		*__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   131
		AxsPub.axis = 
2154
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
   132
            __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);
6bbe93799956 Replaced location for axis ref from %IW(location).0 to %IW(location).402. Added location for network position at %IW(location).
Laurent Bessard
parents: 2153
diff changeset
   133
		AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s;
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   134
%(init_axis_params)s
2118
c317b1aaf920 Fixed bug when using real axis and simulated axis at the same time (got the same index)
Laurent Bessard
parents: 2104
diff changeset
   135
%(fieldbus_interface_definition)s
2045
62e102935f3e Adding test project using higen drive
laurent
parents: 2043
diff changeset
   136
		__FirstTick = 0;
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   137
	}
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   138
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   139
	// Default variables retrieve
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   140
	AxsPub.axis->CommunicationReady = 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   141
        *(AxsPub.StatusWord) != 0;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   142
#define FSA_sep || FSA ==
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   143
    {
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   144
        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   145
        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   146
        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   147
    }
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   148
#undef FSA_sep 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   149
	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   150
	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   151
	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   152
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   153
	// Extra variables retrieve
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   154
%(extra_variables_retrieve)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   155
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   156
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   157
void __publish_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   158
{
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   159
	IEC_BOOL power = 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   160
        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   161
        && AxsPub.axis->Power;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   162
    uint16_t CW = *(AxsPub.ControlWord);
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   163
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   164
#define FSA_sep : case
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
   165
	// CIA402 node state transition computation
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   166
	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   167
	    case SwitchOnDisabled :
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   168
            CW &= ~(SwitchOn | FaultReset);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   169
            CW |= EnableVoltage | QuickStop;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   170
	    	break;
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   171
	    case ReadyToSwitchOn :
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   172
	    case OperationEnabled :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   173
	    	if (!power) {
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   174
                CW &= ~(FaultReset | EnableOperation);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   175
                CW |= SwitchOn | EnableVoltage | QuickStop;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   176
	    		break;
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   177
	    	}
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   178
	    case SwitchedOn :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   179
	    	if (power) {
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   180
                CW &= ~(FaultReset);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   181
                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   182
	    	}
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   183
	    	break;
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   184
	    case Fault :
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   185
            /* TODO reset fault only when MC_Reset */
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   186
            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   187
            CW |= FaultReset;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   188
	    	break;
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   189
	    default:
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   190
	    	break;
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   191
	}
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   192
#undef FSA_sep 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   193
    *(AxsPub.ControlWord) = CW;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   194
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   195
	// CIA402 node modes of operation computation according to axis motion mode
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   196
	switch (AxsPub.axis->AxisMotionMode) {
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   197
		case mc_mode_cst:
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   198
			*(AxsPub.ModesOfOperation) = 0x0a;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   199
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   200
		case mc_mode_csv:
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   201
			*(AxsPub.ModesOfOperation) = 0x09;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   202
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   203
		default:
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   204
			*(AxsPub.ModesOfOperation) = 0x08;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   205
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   206
	}
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   207
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   208
	// Default variables publish
2150
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   209
	*(AxsPub.TargetPosition) = 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   210
            AxsPub.axis->RawPositionSetPoint;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   211
	*(AxsPub.TargetVelocity) = 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   212
            AxsPub.axis->RawVelocitySetPoint;
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   213
	*(AxsPub.TargetTorque) = 
08514552944f Moved ratio scaling back to MCL. Rewrote CiA402 C part in a way it can be read and understood by some humans.
Edouard Tisserant
parents: 2136
diff changeset
   214
            AxsPub.axis->RawTorqueSetPoint;
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   215
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   216
	// Extra variables publish
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   217
%(extra_variables_publish)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   218
}