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

	*(__CIA402Node_%(location)s.ModesOfOperation) = 0x08;

    52

    return 0;

    53

}

    54

    55

void __cleanup_%(location)s()

    56

{

    57

}

    58

    59

void __retrieve_%(location)s()

    60

{

    61

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

    62

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

    63

    64

	if (__FirstTick) {

    65

%(init_axis_params)s

    66

		__FirstTick = 0;

    67

	}

    68

    69

	// CIA402 node state computation

    70

	__CIA402Node_%(location)s.state = __Unknown;

    71

	switch (statusword_inactive) {

    72

		case 0x00:

    73

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    74

			break;

    75

		case 0x40:

    76

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    77

			break;

    78

		case 0x0f:

    79

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    80

			break;

    81

		case 0x08:

    82

			__CIA402Node_%(location)s.state = __Fault;

    83

			break;

    84

		default:

    85

			break;

    86

	}

    87

	switch (statusword_active) {

    88

		case 0x21:

    89

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

    90

			break;

    91

		case 0x23:

    92

			__CIA402Node_%(location)s.state = __SwitchedOn;

    93

			break;

    94

		case 0x27:

    95

			__CIA402Node_%(location)s.state = __OperationEnabled;

    96

			break;

    97

		case 0x07:

    98

			__CIA402Node_%(location)s.state = __QuickStopActive;

    99

			break;

   100

		default:

   101

			break;

   102

	}

   103

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

   104

		return;

   105

	}

   106

   107

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

   108

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

   109

}

   110

   111

void __publish_%(location)s()

   112

{

   113

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

   114

   115

	// CIA402 node state transition computation

   116

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

   117

	    case __SwitchOnDisabled:

   118

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

   119

	    	break;

   120

	    case __ReadyToSwitchOn:

   121

	    case __OperationEnabled:

   122

	    	if (!power) {

   123

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

   124

	    		break;

   125

	    	}

   126

	    case __SwitchedOn:

   127

	    	if (power) {

   128

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

   129

	    	}

   130

	    	break;

   131

	    case __Fault:

   132

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

   133

	    	break;

   134

	    default:

   135

	    	break;

   136

	}

   137

   138

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

   139

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

   140

	}

   141

	else {

   142

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

   143

	}

   144

}