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

    59

IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s;

    60

IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s;

    61

IEC_INT beremiz__IW%(location_str)s_402;

    62

IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402;

    63

    64

%(MCL_headers)s

    65

    66

static IEC_BOOL __FirstTick = 1;

    67

    68

typedef struct {

    69

%(entry_variables)s

    70

    axis_s* axis;

    71

} __CIA402Node;

    72

    73

#define AxsPub __CIA402Node_%(location_str)s

    74

    75

static __CIA402Node AxsPub;

    76

    77

%(extern_located_variables_declaration)s

    78

    79

%(fieldbus_interface_declaration)s

    80

    81

int __init_%(location_str)s()

    82

{

    83

    __FirstTick = 1;

    84

%(init_entry_variables)s

    85

	*(AxsPub.ModesOfOperation) = 0x08;

    86

    return 0;

    87

}

    88

    89

void __cleanup_%(location_str)s()

    90

{

    91

}

    92

    93

void __retrieve_%(location_str)s()

    94

{

    95

	if (__FirstTick) {

    96

		*__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();

    97

		AxsPub.axis = 

    98

            __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);

    99

		AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s;

   100

%(init_axis_params)s

   101

%(fieldbus_interface_definition)s

   102

		__FirstTick = 0;

   103

	}

   104

   105

	// Default variables retrieve

   106

	AxsPub.axis->CommunicationReady = 

   107

        *(AxsPub.StatusWord) != 0;

   108

#define FSA_sep || FSA ==

   109

    {

   110

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

   111

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

   112

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

   113

    }

   114

#undef FSA_sep 

   115

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

   116

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

   117

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

   118

   119

	// Extra variables retrieve

   120

%(extra_variables_retrieve)s

   121

}

   122

   123

void __publish_%(location_str)s()

   124

{

   125

	IEC_BOOL power = 

   126

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

   127

        && AxsPub.axis->Power;

   128

    uint16_t CW = *(AxsPub.ControlWord);

   129

   130

#define FSA_sep : case

   131

	// CIA402 node state transition computation

   132

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

   133

	    case SwitchOnDisabled :

   134

            CW &= ~(SwitchOn | FaultReset);

   135

            CW |= EnableVoltage | QuickStop;

   136

	    	break;

   137

	    case ReadyToSwitchOn :

   138

	    case OperationEnabled :

   139

	    	if (!power) {

   140

                CW &= ~(FaultReset | EnableOperation);

   141

                CW |= SwitchOn | EnableVoltage | QuickStop;

   142

	    		break;

   143

	    	}

   144

	    case SwitchedOn :

   145

	    	if (power) {

   146

                CW &= ~(FaultReset);

   147

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   148

	    	}

   149

	    	break;

   150

	    case Fault :

   151

            /* TODO reset fault only when MC_Reset */

   152

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

   153

            CW |= FaultReset;

   154

	    	break;

   155

	    default:

   156

	    	break;

   157

	}

   158

#undef FSA_sep 

   159

    *(AxsPub.ControlWord) = CW;

   160

   161

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

   162

	switch (AxsPub.axis->AxisMotionMode) {

   163

		case mc_mode_cst:

   164

			*(AxsPub.ModesOfOperation) = 0x0a;

   165

			break;

   166

		case mc_mode_csv:

   167

			*(AxsPub.ModesOfOperation) = 0x09;

   168

			break;

   169

		default:

   170

			*(AxsPub.ModesOfOperation) = 0x08;

   171

			break;

   172

	}

   173

   174

	// Default variables publish

   175

	*(AxsPub.TargetPosition) = 

   176

            AxsPub.axis->RawPositionSetPoint;

   177

	*(AxsPub.TargetVelocity) = 

   178

            AxsPub.axis->RawVelocitySetPoint;

   179

	*(AxsPub.TargetTorque) = 

   180

            AxsPub.axis->RawTorqueSetPoint;

   181

   182

	// Extra variables publish

   183

%(extra_variables_publish)s

   184

}