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

    16

IEC_INT beremiz__IW%(location)s_0;

    17

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

    18

    19

%(MCL_headers)s

    20

    21

static IEC_UINT __InactiveMask = 0x4f;

    22

static IEC_UINT __ActiveMask = 0x6f;

    23

static IEC_UINT __PowerMask = 0x10;

    24

static IEC_BOOL __FirstTick = 1;

    25

    26

typedef enum {

    27

	__Unknown,

    28

	__NotReadyToSwitchOn,

    29

	__SwitchOnDisabled,

    30

	__ReadyToSwitchOn,

    31

	__SwitchedOn,

    32

	__OperationEnabled,

    33

	__QuickStopActive,

    34

    __FaultReactionActive,

    35

    __Fault,

    36

} __CIA402NodeState;

    37

    38

typedef struct {

    39

%(entry_variables)s

    40

    __CIA402NodeState state;

    41

    axis_s* axis;

    42

} __CIA402Node;

    43

    44

#define AXIS_UNIT_TO_USER_UNIT(param)\

    45

(IEC_LREAL)(param) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator

    46

#define USER_UNIT_TO_AXIS_UNIT(param)\

    47

(IEC_DINT)(param * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator)

    48

    49

static __CIA402Node __CIA402Node_%(location)s;

    50

    51

%(extern_located_variables_declaration)s

    52

    55

int __init_%(location)s()

    56

{

    57

%(init_entry_variables)s

    58

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

    59

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

    61

	__MK_Set_AXIS_REF_Pos(*__IW%(location)s_0, %(slave_pos)d);

    62

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

    63

    return 0;

    64

}

    65

    66

void __cleanup_%(location)s()

    67

{

    68

}

    69

    70

void __retrieve_%(location)s()

    71

{

    72

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

    73

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

    74

    75

	if (__FirstTick) {

    76

%(init_axis_params)s

    77

		__FirstTick = 0;

    78

	}

    79

    80

	// CIA402 node state computation

    81

	__CIA402Node_%(location)s.state = __Unknown;

    82

	switch (statusword_inactive) {

    83

		case 0x00:

    84

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    85

			break;

    86

		case 0x40:

    87

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    88

			break;

    89

		case 0x0f:

    90

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    91

			break;

    92

		case 0x08:

    93

			__CIA402Node_%(location)s.state = __Fault;

    94

			break;

    95

		default:

    96

			break;

    97

	}

    98

	switch (statusword_active) {

    99

		case 0x21:

   100

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

   101

			break;

   102

		case 0x23:

   103

			__CIA402Node_%(location)s.state = __SwitchedOn;

   104

			break;

   105

		case 0x27:

   106

			__CIA402Node_%(location)s.state = __OperationEnabled;

   107

			break;

   108

		case 0x07:

   109

			__CIA402Node_%(location)s.state = __QuickStopActive;

   110

			break;

   111

		default:

   112

			break;

   113

	}

   114

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

   115

		return;

   116

	}

   117

   118

	// Default variables retrieve

   119

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

   120

	__CIA402Node_%(location)s.axis->ActualPosition = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition));

   121

	__CIA402Node_%(location)s.axis->ActualVelocity = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity));

   122

   123

	// Extra variables retrieve

   124

%(extra_variables_retrieve)s

   125

}

   126

   127

void __publish_%(location)s()

   128

{

   129

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

   130

   131

	// CIA402 node state transition computation

   132

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

   133

	    case __SwitchOnDisabled:

   134

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

   135

	    	break;

   136

	    case __ReadyToSwitchOn:

   137

	    case __OperationEnabled:

   138

	    	if (!power) {

   139

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

   140

	    		break;

   141

	    	}

   142

	    case __SwitchedOn:

   143

	    	if (power) {

   144

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

   145

	    	}

   146

	    	break;

   147

	    case __Fault:

   148

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

   149

	    	break;

   150

	    default:

   151

	    	break;

   152

	}

   153

   154

	// Default variables publish

   155

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

   156

		*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);

   157

	}

   158

	else {

   159

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

   160

	}

   161

   162

	// Extra variables publish

   163

%(extra_variables_publish)s

   164

}