etherlab/plc_cia402node.c
author Edouard Tisserant
Fri, 09 Jul 2021 15:47:43 +0200
branchsvghmi
changeset 3271 561dbd1e3e04
parent 2643 b98d9e08231f
permissions -rw-r--r--
SVGHMI: Fixing last commit's multiclient implementation, in case of watchdog. To be continued, since multiclient still fail...
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
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
     6
                         RTES Lab : CRKim, JBLee, youcu
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
     7
                         Higen Motor : Donggu Kang
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
     8
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
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
    10
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
    11
(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
    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
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
    14
02a2b5dee5e3 Merged GPL + LGPL v2 or later EtherCAT extension. Fixed headers and copyright notice.
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2154
diff changeset
    15
*/
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    16
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    17
#include "ecrt.h"
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    18
2122
03cf08190ac4 Fixed header files inclusion in CIA402 node template file
Laurent Bessard
parents: 2118
diff changeset
    19
#include "beremiz.h"
03cf08190ac4 Fixed header files inclusion in CIA402 node template file
Laurent Bessard
parents: 2118
diff changeset
    20
#include "iec_types_all.h"
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
    21
2094
72b74099b873 Fix bug in support for Etherlab specific fieldbus interface function blocks
Laurent Bessard
parents: 2092
diff changeset
    22
#include "accessor.h"
2092
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    23
#include "POUS.h"
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
    24
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
    25
/* 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
    26
08514552944f Moved ratio scaling back 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
        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
    28
    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
    29
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
    30
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
    31
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
    32
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
    33
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
    34
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
    35
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
    36
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
    37
*/
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    38
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    39
//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    40
/* From CiA402, Page 63 Statusword for homing mode
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    41
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    42
		Table 106 - Definition of bit 10, bit 12, bit 13
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    43
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    44
xx00 x0xx xxxx xxxx | Homing procedure is in progress
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    45
xx00 x1xx xxxx xxxx | Homing procedure is interrupted or not started
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    46
xx01 x0xx xxxx xxxx | Homing is attained, but target is not reached
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    47
xx01 x1xx xxxx xxxx | Homing procedure is completed successfully
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    48
xx10 x0xx xxxx xxxx | Homing error occurred, velocity is not 0
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    49
xx10 x1xx xxxx xxxx | Homing error occurred, velocity is 0
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    50
xx11 xxxx xxxx xxxx | reserved
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    51
*/
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    52
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
    53
#define FSAFromStatusWord(SW) (SW & 0x006f)
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    54
//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    55
#define HomingStatusWord(SW) (SW & 0x3400)
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    56
#define FaultFromStatusWord(SW) (SW & 0x0008)
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
    57
#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
    58
#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
    59
#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
    60
#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
    61
#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
    62
#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
    63
#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
    64
#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
    65
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    66
//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    67
#define HomingInProgress	0b0000000000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    68
#define HomingNotRunning	0b0000010000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    69
#define HomingNotReached	0b0001000000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    70
#define Homing_Completed	0b0001010000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    71
#define HomingErrorInVelo	0b0010000000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    72
#define HomingErrorNotVelo	0b0010010000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    73
#define HomingReserved		0b0011000000000000 FSA_sep 0b0011010000000000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    74
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    75
// StatusWord bits :
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
    76
#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
    77
#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
    78
#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
    79
#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
    80
#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
    81
#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
    82
#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
    83
#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
    84
#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
    85
#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
    86
#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
    87
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    88
//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    89
#define SW_HomingAttained		0x1000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    90
#define SW_HomingError			0x2000
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
    91
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
    92
// 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
    93
#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
    94
#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
    95
#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
    96
#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
    97
#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
    98
#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
    99
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   100
//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   101
//#define Homing_OperationStart 0x0010
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   102
#define Homing_OperationStart_Origin 0x0010
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   103
#define Homing_OperationStart_Edit 0x001F
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   104
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   105
IEC_INT beremiz__IW%(location)s = %(slave_pos)s;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   106
IEC_INT *__IW%(location)s = &beremiz__IW%(location)s;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   107
IEC_INT beremiz__IW%(location)s_402;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   108
IEC_INT *__IW%(location)s_402 = &beremiz__IW%(location)s_402;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   109
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   110
%(MCL_headers)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   111
2059
b74955d211c7 fixed multiple cia402 instance symbol collision
Edouard Tisserant
parents: 2045
diff changeset
   112
static IEC_BOOL __FirstTick = 1;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   113
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
   114
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
   115
    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
   116
    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
   117
    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
   118
    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
   119
} 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
   120
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
   121
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
   122
   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
   123
   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
   124
   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
   125
   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
   126
   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
   127
   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
   128
   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
   129
   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
   130
   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
   131
   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
   132
   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
   133
   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
   134
   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
   135
   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
   136
   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
   137
}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
   138
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   139
typedef struct {
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   140
%(entry_variables)s
2077
14dfb786c53e Fix bug when modifying slave position
Laurent Bessard
parents: 2070
diff changeset
   141
    axis_s* axis;
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
   142
} __CIA402Node;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   143
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   144
#define AxsPub __CIA402Node_%(location)s
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   145
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
static __CIA402Node AxsPub;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   147
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   148
%(extern_located_variables_declaration)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   149
2092
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
   150
%(fieldbus_interface_declaration)s
c9776ae8b5d0 Adding support for Etherlab specific function blocks for MCL fieldbus interface function blocks
Laurent Bessard
parents: 2091
diff changeset
   151
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   152
int __init_%(location)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   153
{
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
   154
    __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
   155
%(init_entry_variables)s
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
   156
    return 0;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   157
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   158
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   159
void __cleanup_%(location)s()
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   160
{
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   161
}
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   162
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   163
void __retrieve_%(location)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   164
{
2041
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   165
	if (__FirstTick) {
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   166
		*__IW%(location)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
   167
		AxsPub.axis = 
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   168
            __MK_GetPublic_AXIS_REF(*__IW%(location)s_402);
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   169
		AxsPub.axis->NetworkPosition = beremiz__IW%(location)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
   170
%(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
   171
%(fieldbus_interface_definition)s
2045
62e102935f3e Adding test project using higen drive
laurent
parents: 2043
diff changeset
   172
		__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
   173
	}
ce3727171207 Defining all slaves as Etherlab master subplugin instead of editing them in an editor with vertical notebook
laurent
parents: 2036
diff changeset
   174
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   175
	// 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
   176
	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
   177
        *(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
   178
#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
   179
    {
08514552944f Moved ratio scaling back 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
        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
   181
        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
   182
        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
   183
    }
08514552944f Moved ratio scaling back 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
#undef FSA_sep 
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   185
%(default_variables_retrieve)s
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   186
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   187
	// Extra variables retrieve
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   188
%(extra_variables_retrieve)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   189
}
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   190
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   191
void __publish_%(location)s()
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   192
{
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
   193
	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
   194
        ((*(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
   195
        && 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
   196
    uint16_t CW = *(AxsPub.ControlWord);
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   197
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
#define FSA_sep : case
2043
27d4cb689a79 Adding plugin icons and replacing reference to DS402 by CIA402
laurent
parents: 2041
diff changeset
   199
	// 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
   200
	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
   201
	    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
   202
            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
   203
            CW |= EnableVoltage | QuickStop;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   204
	    	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
   205
	    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
   206
	    case OperationEnabled :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   207
	    	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
   208
                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
   209
                CW |= SwitchOn | EnableVoltage | QuickStop;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   210
	    		break;
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   211
	    	}
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
   212
	    case SwitchedOn :
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   213
	    	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
   214
                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
   215
                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
2034
ae8fecf082a1 Adding support for MCL
laurent
parents: 2032
diff changeset
   216
	    	}
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   217
	    	break;
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   218
			//ssh_check
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   219
//	    case Fault :
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   220
//            /* TODO reset fault only when MC_Reset */
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   221
//	    	AxsPub.axis->DriveFault = 1;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   222
//            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   223
//            CW |= FaultReset;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   224
//	    	break;
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   225
	    default:
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   226
	    	break;
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   227
	}
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   228
	//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   229
	if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault)
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   230
		AxsPub.axis->DriveFault = 1;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   231
	else{
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   232
		AxsPub.axis->DriveFault = 0;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   233
		AxsPub.axis->DriveFaultReset = 0;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   234
	}
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   235
	if(AxsPub.axis->DriveFaultReset){
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   236
		CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   237
		CW |= FaultReset;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   238
	}
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   239
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   240
	//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   241
	switch (HomingStatusWord(*(AxsPub.StatusWord))) {
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   242
		case HomingInProgress:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   243
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   244
		case HomingNotRunning:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   245
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   246
		case HomingNotReached:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   247
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   248
		case Homing_Completed:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   249
			if(!AxsPub.axis->HomingCompleted)
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   250
				AxsPub.axis->HomingCompleted = 1;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   251
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   252
		case HomingErrorInVelo:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   253
		case HomingErrorNotVelo:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   254
			if(!AxsPub.axis->HomingCompleted)
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   255
				AxsPub.axis->HomingCompleted = 1;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   256
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   257
		case HomingReserved:
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   258
			break;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   259
	}
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
   260
#undef FSA_sep 
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   261
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   262
	//ssh_add
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   263
%(modeofop_homing_method)s
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   264
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   265
	*(AxsPub.ControlWord) = CW;
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   266
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   267
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   268
	// CIA402 node modes of operation computation according to axis motion mode
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   269
%(modeofop_computation_mode)s
2136
71fdfd4a12a3 Added support for using CIA402 motion mode csp, csv and cst
Laurent Bessard
parents: 2128
diff changeset
   270
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   271
	// Default variables publish
2641
c9deff128c37 EtherCat master plugin : commit changes recovered from KOSMOS 2018 installer, unkown author(s).
Edouard Tisserant <edouard.tisserant@gmail.com>
parents: 2165
diff changeset
   272
%(default_variables_publish)s
2084
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   273
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   274
	// Extra variables publish
579af800b359 Adding support to enable CIA402 slave digital inputs
Laurent Bessard
parents: 2082
diff changeset
   275
%(extra_variables_publish)s
2032
766078d83e22 Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
diff changeset
   276
}