1

/*

     2

 * Ethercat DS402 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

    23

typedef enum {

    24

	__Unknown,

    25

	__NotReadyToSwitchOn,

    26

	__SwitchOnDisabled,

    27

	__ReadyToSwitchOn,

    28

	__SwitchedOn,

    29

	__OperationEnabled,

    30

	__QuickStopActive,

    31

	__FaultReactionActive,

    32

	__Fault,

    33

} __DS402NodeState;

    34

    35

typedef struct {

    36

%(entry_variables)s

    37

	__DS402NodeState state;

    38

	axis_s* axis;

    39

} __DS402Node;

    40

    41

static __DS402Node __DS402Node_%(location)s;

    42

    43

%(extern_located_variables_declaration)s

    44

    45

int __init_%(location)s()

    46

{

    47

%(init_entry_variables)s;

    48

*__IW%(location)s_0 = __MK_AllocAxis(&(__DS402Node_%(location)s.axis));

    49

	return 0;

    50

}

    51

    52

void __cleanup_%(location)s()

    53

{

    54

}

    55

    56

void __retrieve_%(location)s()

    57

{

    58

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

    59

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

    60

    61

	// DS402 node state computation

    62

	__DS402Node_%(location)s.state = __Unknown;

    63

	switch (statusword_inactive) {

    64

		case 0x00:

    65

			__DS402Node_%(location)s.state = __NotReadyToSwitchOn;

    66

			break;

    67

		case 0x40:

    68

			__DS402Node_%(location)s.state = __SwitchOnDisabled;

    69

			break;

    70

		case 0x0f:

    71

			__DS402Node_%(location)s.state = __FaultReactionActive;

    72

			break;

    73

		case 0x08:

    74

			__DS402Node_%(location)s.state = __Fault;

    75

			break;

    76

		default:

    77

			break;

    78

	}

    79

	switch (statusword_active) {

    80

		case 0x21:

    81

			__DS402Node_%(location)s.state = __ReadyToSwitchOn;

    82

			break;

    83

		case 0x23:

    84

			__DS402Node_%(location)s.state = __SwitchedOn;

    85

			break;

    86

		case 0x27:

    87

			__DS402Node_%(location)s.state = __OperationEnabled;

    88

			break;

    89

		case 0x07:

    90

			__DS402Node_%(location)s.state = __QuickStopActive;

    91

			break;

    92

		default:

    93

			break;

    94

	}

    95

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

    96

		return;

    97

	}

    98

    99

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

   100

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

   101

   102

	__MK_UpdateAxis(*__IW%(location)s_0);

   103

}

   104

   105

void __publish_%(location)s()

   106

{

   107

	__MK_ComputeAxis(*__IW%(location)s_0);

   108

   109

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

   110

   111

	// DS402 node state transition computation

   112

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

   113

	    case __SwitchOnDisabled:

   114

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

   115

	    	break;

   116

	    case __ReadyToSwitchOn:

   117

	    case __OperationEnabled:

   118

	    	if (!power) {

   119

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

   120

	    		break;

   121

	    	}

   122

	    case __SwitchedOn:

   123

	    	if (power) {

   124

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

   125

	    	}

   126

	    	break;

   127

	    case __Fault:

   128

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

   129

	    	break;

   130

	    default:

   131

	    	break;

   132

	}

   133

   134

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

   135

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

   136

	}

   137

	else {

   138

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

   139

	}

   140

}