etherlab/plc_cia402node.c
author Edouard Tisserant <edouard.tisserant@gmail.com>
Sat, 04 Apr 2020 22:32:54 +0200
branchsvghmi
changeset 2920 3ee337c8c769
parent 2491 362039519454
child 2643 b98d9e08231f
permissions -rw-r--r--
SVGHMI: finished shift and capslock support n keypad widget. Added a helper in widgets_common to collect subelements likle active/inactive/disabled...
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
}