etherlab/plc_cia402node.c
author Mario de Sousa <msousa@fe.up.pt>
Sun, 19 Feb 2023 08:37:27 +0000
changeset 3733 d1acf20e8e7c
parent 2643 b98d9e08231f
permissions -rw-r--r--
Modbus plugin: add optional delay between two consecutive modbus requests sent by master/client
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
}