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

#include "accessor.h"

    15

#include "POUS.h"

    16

    17

IEC_INT beremiz__IW%(location)s_0;

    18

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

    19

    20

%(MCL_headers)s

    21

    22

static IEC_UINT __InactiveMask = 0x4f;

    23

static IEC_UINT __ActiveMask = 0x6f;

    24

static IEC_UINT __PowerMask = 0x10;

    25

static IEC_BOOL __FirstTick = 1;

    26

    27

typedef enum {

    28

	__Unknown,

    29

	__NotReadyToSwitchOn,

    30

	__SwitchOnDisabled,

    31

	__ReadyToSwitchOn,

    32

	__SwitchedOn,

    33

	__OperationEnabled,

    34

	__QuickStopActive,

    35

    __FaultReactionActive,

    36

    __Fault,

    37

} __CIA402NodeState;

    38

    39

typedef struct {

    40

%(entry_variables)s

    41

    __CIA402NodeState state;

    42

    axis_s* axis;

    43

} __CIA402Node;

    44

    45

#define AXIS_UNIT_TO_USER_UNIT(param)\

    46

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

    47

#define USER_UNIT_TO_AXIS_UNIT(param)\

    48

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

    49

    50

static __CIA402Node __CIA402Node_%(location)s;

    51

    52

%(extern_located_variables_declaration)s

    53

    54

%(fieldbus_interface_declaration)s

    55

    56

int __init_%(location)s()

    57

{

    59

%(init_entry_variables)s

    60

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

    61

    return 0;

    62

}

    63

    64

void __cleanup_%(location)s()

    65

{

    66

}

    67

    68

void __retrieve_%(location)s()

    69

{

    70

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

    71

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

    72

    73

	if (__FirstTick) {

    77

%(init_axis_params)s

    79

		__FirstTick = 0;

    80

	}

    81

    82

	// CIA402 node state computation

    83

	__CIA402Node_%(location)s.state = __Unknown;

    84

	switch (statusword_inactive) {

    85

		case 0x00:

    86

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    87

			break;

    88

		case 0x40:

    89

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    90

			break;

    91

		case 0x0f:

    92

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    93

			break;

    94

		case 0x08:

    95

			__CIA402Node_%(location)s.state = __Fault;

    96

			break;

    97

		default:

    98

			break;

    99

	}

   100

	switch (statusword_active) {

   101

		case 0x21:

   102

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

   103

			break;

   104

		case 0x23:

   105

			__CIA402Node_%(location)s.state = __SwitchedOn;

   106

			break;

   107

		case 0x27:

   108

			__CIA402Node_%(location)s.state = __OperationEnabled;

   109

			break;

   110

		case 0x07:

   111

			__CIA402Node_%(location)s.state = __QuickStopActive;

   112

			break;

   113

		default:

   114

			break;

   115

	}

   116

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

   117

		return;

   118

	}

   119

   120

	// Default variables retrieve

   121

	__CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0;

   122

	__CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled;

   123

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

   124

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

   125

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

   126

   127

	// Extra variables retrieve

   128

%(extra_variables_retrieve)s

   129

}

   130

   131

void __publish_%(location)s()

   132

{

   133

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

   134

   135

	// CIA402 node state transition computation

   136

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

   137

	    case __SwitchOnDisabled:

   138

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

   139

	    	break;

   140

	    case __ReadyToSwitchOn:

   141

	    case __OperationEnabled:

   142

	    	if (!power) {

   143

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

   144

	    		break;

   145

	    	}

   146

	    case __SwitchedOn:

   147

	    	if (power) {

   148

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

   149

	    	}

   150

	    	break;

   151

	    case __Fault:

   152

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

   153

	    	break;

   154

	    default:

   155

	    	break;

   156

	}

   157

   158

	// Default variables publish

   159

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

   160

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

   161

	}

   162

	else {

   163

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

   164

	}

   165

   166

	// Extra variables publish

   167

%(extra_variables_publish)s

   168

}