1

/*

     2

 * Ethercat CIA402 node execution code

     3

 *

     4

 * */

     5

     6

#include "ecrt.h"

     7

    10

    11

#include "accessor.h"

    12

#include "POUS.h"

    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

#define AXIS_UNIT_TO_USER_UNIT(param)\

    43

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

    44

#define USER_UNIT_TO_AXIS_UNIT(param)\

    45

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

    46

    47

static __CIA402Node __CIA402Node_%(location)s;

    48

    49

%(extern_located_variables_declaration)s

    50

    51

%(fieldbus_interface_declaration)s

    52

    53

int __init_%(location)s()

    54

{

    55

    __FirstTick = 1;

    56

%(init_entry_variables)s

    57

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

    58

    return 0;

    59

}

    60

    61

void __cleanup_%(location)s()

    62

{

    63

}

    64

    65

void __retrieve_%(location)s()

    66

{

    67

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

    68

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

    69

    70

	if (__FirstTick) {

    71

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

    72

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

    73

		__CIA402Node_%(location)s.axis->NetworkPosition = %(slave_pos)d;

    74

%(init_axis_params)s

    75

%(fieldbus_interface_definition)s

    76

		__FirstTick = 0;

    77

	}

    78

    79

	// CIA402 node state computation

    80

	__CIA402Node_%(location)s.state = __Unknown;

    81

	switch (statusword_inactive) {

    82

		case 0x00:

    83

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    84

			break;

    85

		case 0x40:

    86

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    87

			break;

    88

		case 0x0f:

    89

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    90

			break;

    91

		case 0x08:

    92

			__CIA402Node_%(location)s.state = __Fault;

    93

			break;

    94

		default:

    95

			break;

    96

	}

    97

	switch (statusword_active) {

    98

		case 0x21:

    99

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

   100

			break;

   101

		case 0x23:

   102

			__CIA402Node_%(location)s.state = __SwitchedOn;

   103

			break;

   104

		case 0x27:

   105

			__CIA402Node_%(location)s.state = __OperationEnabled;

   106

			break;

   107

		case 0x07:

   108

			__CIA402Node_%(location)s.state = __QuickStopActive;

   109

			break;

   110

		default:

   111

			break;

   112

	}

   113

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

   114

		return;

   115

	}

   116

   117

	// Default variables retrieve

   118

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

   119

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

   120

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

   121

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

   122

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

   123

   124

	// Extra variables retrieve

   125

%(extra_variables_retrieve)s

   126

}

   127

   128

void __publish_%(location)s()

   129

{

   130

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

   131

   132

	// CIA402 node state transition computation

   133

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

   134

	    case __SwitchOnDisabled:

   135

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

   136

	    	break;

   137

	    case __ReadyToSwitchOn:

   138

	    case __OperationEnabled:

   139

	    	if (!power) {

   140

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

   141

	    		break;

   142

	    	}

   143

	    case __SwitchedOn:

   144

	    	if (power) {

   145

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

   146

	    	}

   147

	    	break;

   148

	    case __Fault:

   149

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

   150

	    	break;

   151

	    default:

   152

	    	break;

   153

	}

   154

   155

	// Default variables publish

   156

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

   157

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

   158

	}

   159

	else {

   160

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

   161

	}

   162

   163

	// Extra variables publish

   164

%(extra_variables_publish)s

   165

}