1

/*

     2

 * Ethercat CIA402 node execution code

     3

 *

     4

 * */

     5

     6

#include "ecrt.h"

     7

     8

#ifdef _WINDOWS_H

     9

  #include "iec_types.h"

    10

#else

    11

  #include "iec_std_lib.h"

    12

#endif

    13

    14

IEC_INT beremiz__IW%(location)s_0;

    15

IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0;

    16

    17

%(MCL_headers)s

    18

    19

static IEC_UINT __InactiveMask = 0x4f;

    20

static IEC_UINT __ActiveMask = 0x6f;

    21

static IEC_UINT __PowerMask = 0x10;

    22

static IEC_BOOL __FirstTick = 1;

    23

    24

typedef enum {

    25

	__Unknown,

    26

	__NotReadyToSwitchOn,

    27

	__SwitchOnDisabled,

    28

	__ReadyToSwitchOn,

    29

	__SwitchedOn,

    30

	__OperationEnabled,

    31

	__QuickStopActive,

    32

	__FaultReactionActive,

    33

	__Fault,

    34

} __CIA402NodeState;

    35

    36

typedef struct {

    37

%(entry_variables)s

    38

	__CIA402NodeState state;

    39

	axis_s* axis;

    40

} __CIA402Node;

    41

    42

static __CIA402Node __CIA402Node_%(location)s;

    43

    44

%(extern_located_variables_declaration)s

    45

    46

int __init_%(location)s()

    47

{

    48

%(init_entry_variables)s

    49

	*__IW%(location)s_0 = __MK_Alloc_AXIS_REF();

    50

	__CIA402Node_%(location)s.axis = __MK_GetPublic_AXIS_REF(*__IW%(location)s_0);

    51

	return 0;

    52

}

    53

    54

void __cleanup_%(location)s()

    55

{

    56

}

    57

    58

void __retrieve_%(location)s()

    59

{

    60

	IEC_UINT statusword_inactive = *(__CIA402Node_%(location)s.StatusWord) & __InactiveMask;

    61

	IEC_UINT statusword_active = *(__CIA402Node_%(location)s.StatusWord) & __ActiveMask;

    62

    63

	if (__FirstTick) {

    64

%(init_axis_params)s

    65

		__FirstTick = 0;

    66

	}

    67

    68

	// CIA402 node state computation

    69

	__CIA402Node_%(location)s.state = __Unknown;

    70

	switch (statusword_inactive) {

    71

		case 0x00:

    72

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    73

			break;

    74

		case 0x40:

    75

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    76

			break;

    77

		case 0x0f:

    78

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    79

			break;

    80

		case 0x08:

    81

			__CIA402Node_%(location)s.state = __Fault;

    82

			break;

    83

		default:

    84

			break;

    85

	}

    86

	switch (statusword_active) {

    87

		case 0x21:

    88

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

    89

			break;

    90

		case 0x23:

    91

			__CIA402Node_%(location)s.state = __SwitchedOn;

    92

			break;

    93

		case 0x27:

    94

			__CIA402Node_%(location)s.state = __OperationEnabled;

    95

			break;

    96

		case 0x07:

    97

			__CIA402Node_%(location)s.state = __QuickStopActive;

    98

			break;

    99

		default:

   100

			break;

   101

	}

   102

	if (__CIA402Node_%(location)s.state == __Unknown) {

   103

		return;

   104

	}

   105

   106

	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;

   107

	__CIA402Node_%(location)s.axis->ActualPosition = (IEC_REAL)(*(__CIA402Node_%(location)s.ActualPosition)) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator;

   108

}

   109

   110

void __publish_%(location)s()

   111

{

   112

	IEC_BOOL power = ((*(__CIA402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __CIA402Node_%(location)s.axis->Power;

   113

   114

	// CIA402 node state transition computation

   115

	switch (__CIA402Node_%(location)s.state) {

   116

	    case __SwitchOnDisabled:

   117

	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x87) | 0x06;

   118

	    	break;

   119

	    case __ReadyToSwitchOn:

   120

	    case __OperationEnabled:

   121

	    	if (!power) {

   122

	    		*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x07;

   123

	    		break;

   124

	    	}

   125

	    case __SwitchedOn:

   126

	    	if (power) {

   127

	    	    *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f;

   128

	    	}

   129

	    	break;

   130

	    case __Fault:

   131

	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x80;

   132

	    	break;

   133

	    default:

   134

	    	break;

   135

	}

   136

   137

	if (__CIA402Node_%(location)s.axis->CSP && *(__CIA402Node_%(location)s.ModesOfOperationDisplay) == 0x08) {

   138

		*(__CIA402Node_%(location)s.TargetPosition) = (IEC_DINT)(__CIA402Node_%(location)s.axis->PositionSetPoint * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator);

   139

	}

   140

	else {

   141

		*(__CIA402Node_%(location)s.TargetPosition) = *(__CIA402Node_%(location)s.ActualPosition);

   142

	}

   143

}