etherlab/plc_cia402node.c
author Andrey Skvortsov <andrej.skvortzov@gmail.com>
Wed, 26 Sep 2018 14:15:22 +0300
changeset 2345 a56362c818a3
parent 2165 02a2b5dee5e3
child 2491 362039519454
child 2641 c9deff128c37
permissions -rw-r--r--
use the same font size on Windows and GNU/Linux
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
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    77
typedef struct {
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    78
%(entry_variables)s
2077
14dfb786c53e Fix bug when modifying slave position
Laurent Bessard
parents: 2070
diff changeset
    79
    axis_s* axis;
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
    80
} __CIA402Node;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    81
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
    82
#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
    83
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
    84
static __CIA402Node AxsPub;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    85
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
    86
%(extern_located_variables_declaration)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    87
2092
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    88
%(fieldbus_interface_declaration)s
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    89
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
    90
int __init_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    91
{
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
    92
    __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
    93
%(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
    94
	*(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
    95
    return 0;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    96
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    97
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
    98
void __cleanup_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    99
{
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   100
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   101
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   102
void __retrieve_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   103
{
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   104
	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
   105
		*__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
   106
		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
   107
            __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
   108
		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
   109
%(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
   110
%(fieldbus_interface_definition)s
2045
62e102935f3e Adding test project using higen drive
laurent
parents: 2043
diff changeset
   111
		__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
   112
	}
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   113
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   114
	// 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
   115
	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
   116
        *(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
   117
#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
   118
    {
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
        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
   120
        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
   121
        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
   122
    }
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
   123
#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
   124
	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
   125
	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
   126
	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   127
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   128
	// Extra variables retrieve
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   129
%(extra_variables_retrieve)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   130
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   131
2153
91c10856adaa Rewrite CIA402 node specific code generating part
Laurent Bessard
parents: 2150
diff changeset
   132
void __publish_%(location_str)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   133
{
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
   134
	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
   135
        ((*(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
   136
        && 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
   137
    uint16_t CW = *(AxsPub.ControlWord);
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   138
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
   139
#define FSA_sep : case
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
   140
	// 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
   141
	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
   142
	    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
   143
            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
   144
            CW |= EnableVoltage | QuickStop;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   145
	    	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
   146
	    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
   147
	    case OperationEnabled :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   148
	    	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
   149
                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
   150
                CW |= SwitchOn | EnableVoltage | QuickStop;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   151
	    		break;
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   152
	    	}
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
   153
	    case SwitchedOn :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   154
	    	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
   155
                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
   156
                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   157
	    	}
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   158
	    	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
   159
	    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
   160
            /* 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
   161
            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
   162
            CW |= FaultReset;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   163
	    	break;
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   164
	    default:
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   165
	    	break;
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   166
	}
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
   167
#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
   168
    *(AxsPub.ControlWord) = CW;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   169
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   170
	// 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
   171
	switch (AxsPub.axis->AxisMotionMode) {
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   172
		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
   173
			*(AxsPub.ModesOfOperation) = 0x0a;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   174
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   175
		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
   176
			*(AxsPub.ModesOfOperation) = 0x09;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   177
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   178
		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
   179
			*(AxsPub.ModesOfOperation) = 0x08;
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   180
			break;
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   181
	}
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   182
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   183
	// 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
   184
	*(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
   185
            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
   186
	*(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
   187
            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
   188
	*(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
   189
            AxsPub.axis->RawTorqueSetPoint;
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   190
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   191
	// Extra variables publish
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   192
%(extra_variables_publish)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   193
}