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

IEC_UINT __InactiveMask = 0x4f;

    20

IEC_UINT __ActiveMask = 0x6f;

    21

IEC_UINT __PowerMask = 0x10;

    22

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_AllocAxis(&(__CIA402Node_%(location)s.axis));

    50

	return 0;

    51

}

    52

    53

void __cleanup_%(location)s()

    54

{

    55

}

    56

    57

void __retrieve_%(location)s()

    58

{

    59

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

    60

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

    61

    62

	if (__FirstTick) {

    63

%(init_axis_params)s

    64

		__FirstTick = 0;

    65

	}

    66

    67

	// CIA402 node state computation

    68

	__CIA402Node_%(location)s.state = __Unknown;

    69

	switch (statusword_inactive) {

    70

		case 0x00:

    71

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    72

			break;

    73

		case 0x40:

    74

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    75

			break;

    76

		case 0x0f:

    77

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    78

			break;

    79

		case 0x08:

    80

			__CIA402Node_%(location)s.state = __Fault;

    81

			break;

    82

		default:

    83

			break;

    84

	}

    85

	switch (statusword_active) {

    86

		case 0x21:

    87

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

    88

			break;

    89

		case 0x23:

    90

			__CIA402Node_%(location)s.state = __SwitchedOn;

    91

			break;

    92

		case 0x27:

    93

			__CIA402Node_%(location)s.state = __OperationEnabled;

    94

			break;

    95

		case 0x07:

    96

			__CIA402Node_%(location)s.state = __QuickStopActive;

    97

			break;

    98

		default:

    99

			break;

   100

	}

   101

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

   102

		return;

   103

	}

   104

   105

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

   106

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

   107

   108

	__MK_UpdateAxis(*__IW%(location)s_0);

   109

}

   110

   111

void __publish_%(location)s()

   112

{

   113

	__MK_ComputeAxis(*__IW%(location)s_0);

   114

   115

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

   116

   117

	// CIA402 node state transition computation

   118

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

   119

	    case __SwitchOnDisabled:

   120

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

   121

	    	break;

   122

	    case __ReadyToSwitchOn:

   123

	    case __OperationEnabled:

   124

	    	if (!power) {

   125

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

   126

	    		break;

   127

	    	}

   128

	    case __SwitchedOn:

   129

	    	if (power) {

   130

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

   131

	    	}

   132

	    	break;

   133

	    case __Fault:

   134

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

   135

	    	break;

   136

	    default:

   137

	    	break;

   138

	}

   139

   140

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

   141

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

   142

	}

   143

	else {

   144

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

   145

	}

   146

}