1

/*

     2

 * Ethercat CIA402 node execution code

     3

 *

     4

 * */

     5

     6

#include "ecrt.h"

     7

     8

#include "beremiz.h"

     9

#include "iec_types_all.h"

    10

    11

#include "accessor.h"

    12

#include "POUS.h"

    13

    14

/* From CiA402, page 27

    15

    16

        Table 30 - State coding

    17

    Statusword      |      PDS FSA state

    18

xxxx xxxx x0xx 0000 | Not ready to switch on

    19

xxxx xxxx x1xx 0000 | Switch on disabled

    20

xxxx xxxx x01x 0001 | Ready to switch on

    21

xxxx xxxx x01x 0011 | Switched on

    22

xxxx xxxx x01x 0111 | Operation enabled

    23

xxxx xxxx x00x 0111 | Quick stop active

    24

xxxx xxxx x0xx 1111 | Fault reaction active

    25

xxxx xxxx x0xx 1000 | Fault

    26

*/

    27

#define FSAFromStatusWord(SW) (SW & 0x006f)

    28

#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000

    29

#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000

    30

#define ReadyToSwitchOn     0b00100001

    31

#define SwitchedOn          0b00100011

    32

#define OperationEnabled    0b00100111

    33

#define QuickStopActive     0b00000111

    34

#define FaultReactionActive 0b00001111 FSA_sep 0b00101111

    35

#define Fault               0b00001000 FSA_sep 0b00101000

    36

    37

// SatusWord bits :

    38

#define SW_ReadyToSwitchOn     0x0001

    39

#define SW_SwitchedOn          0x0002

    40

#define SW_OperationEnabled    0x0004

    41

#define SW_Fault               0x0008

    42

#define SW_VoltageEnabled      0x0010

    43

#define SW_QuickStop           0x0020

    44

#define SW_SwitchOnDisabled    0x0040

    45

#define SW_Warning             0x0080

    46

#define SW_Remote              0x0200

    47

#define SW_TargetReached       0x0400

    48

#define SW_InternalLimitActive 0x0800

    49

    50

// ControlWord bits :

    51

#define SwitchOn        0x0001

    52

#define EnableVoltage   0x0002

    53

#define QuickStop       0x0004

    54

#define EnableOperation 0x0008

    55

#define FaultReset      0x0080

    56

#define Halt            0x0100

    57

    58

    61

    62

%(MCL_headers)s

    63

    64

static IEC_BOOL __FirstTick = 1;

    65

    66

typedef struct {

    67

%(entry_variables)s

    68

    axis_s* axis;

    69

} __CIA402Node;

    70

    72

    73

static __CIA402Node AxsPub;

    74

    75

%(extern_located_variables_declaration)s

    76

    77

%(fieldbus_interface_declaration)s

    78

    80

{

    81

    __FirstTick = 1;

    82

%(init_entry_variables)s

    83

	*(AxsPub.ModesOfOperation) = 0x08;

    84

    return 0;

    85

}

    86

    88

{

    89

}

    90

    92

{

    93

	if (__FirstTick) {

    95

		AxsPub.axis = 

    97

		AxsPub.axis->NetworkPosition = %(slave_pos)d;

    98

%(init_axis_params)s

    99

%(fieldbus_interface_definition)s

   100

		__FirstTick = 0;

   101

	}

   102

   103

	// Default variables retrieve

   104

	AxsPub.axis->CommunicationReady = 

   105

        *(AxsPub.StatusWord) != 0;

   106

#define FSA_sep || FSA ==

   107

    {

   108

        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));

   109

        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;

   110

        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;

   111

    }

   112

#undef FSA_sep 

   113

	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);

   114

	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);

   115

	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);

   116

   117

	// Extra variables retrieve

   118

%(extra_variables_retrieve)s

   119

}

   120

   122

{

   123

	IEC_BOOL power = 

   124

        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 

   125

        && AxsPub.axis->Power;

   126

    uint16_t CW = *(AxsPub.ControlWord);

   127

   128

#define FSA_sep : case

   129

	// CIA402 node state transition computation

   130

	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {

   131

	    case SwitchOnDisabled :

   132

            CW &= ~(SwitchOn | FaultReset);

   133

            CW |= EnableVoltage | QuickStop;

   134

	    	break;

   135

	    case ReadyToSwitchOn :

   136

	    case OperationEnabled :

   137

	    	if (!power) {

   138

                CW &= ~(FaultReset | EnableOperation);

   139

                CW |= SwitchOn | EnableVoltage | QuickStop;

   140

	    		break;

   141

	    	}

   142

	    case SwitchedOn :

   143

	    	if (power) {

   144

                CW &= ~(FaultReset);

   145

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   146

	    	}

   147

	    	break;

   148

	    case Fault :

   149

            /* TODO reset fault only when MC_Reset */

   150

            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);

   151

            CW |= FaultReset;

   152

	    	break;

   153

	    default:

   154

	    	break;

   155

	}

   156

#undef FSA_sep 

   157

    *(AxsPub.ControlWord) = CW;

   158

   159

	// CIA402 node modes of operation computation according to axis motion mode

   160

	switch (AxsPub.axis->AxisMotionMode) {

   161

		case mc_mode_cst:

   162

			*(AxsPub.ModesOfOperation) = 0x0a;

   163

			break;

   164

		case mc_mode_csv:

   165

			*(AxsPub.ModesOfOperation) = 0x09;

   166

			break;

   167

		default:

   168

			*(AxsPub.ModesOfOperation) = 0x08;

   169

			break;

   170

	}

   171

   172

	// Default variables publish

   173

	*(AxsPub.TargetPosition) = 

   174

            AxsPub.axis->RawPositionSetPoint;

   175

	*(AxsPub.TargetVelocity) = 

   176

            AxsPub.axis->RawVelocitySetPoint;

   177

	*(AxsPub.TargetTorque) = 

   178

            AxsPub.axis->RawTorqueSetPoint;

   179

   180

	// Extra variables publish

   181

%(extra_variables_publish)s

   182

}