1

/*

     2

 * Ethercat CIA402 node execution code

     3

 *

     4

 * */

     5

     6

#include "ecrt.h"

     7

     8

#include "beremiz.h"

     9

#include "iec_types_all.h"

    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, type, name)\

    43

(IEC_##type)(param) * __CIA402Node_%(location)s.axis->name##RatioDenominator / __CIA402Node_%(location)s.axis->name##RatioNumerator

    44

#define USER_UNIT_TO_AXIS_UNIT(param, type, name)\

    45

(IEC_##type)(param * __CIA402Node_%(location)s.axis->name##RatioNumerator / __CIA402Node_%(location)s.axis->name##RatioDenominator)

    46

    47

#define DEFAULT_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL,)

    48

#define DEFAULT_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, DINT,)

    49

#define TORQUE_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL, Torque)

    50

#define TORQUE_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, INT, Torque)

    51

    52

static __CIA402Node __CIA402Node_%(location)s;

    53

    54

%(extern_located_variables_declaration)s

    55

    56

%(fieldbus_interface_declaration)s

    57

    58

int __init_%(location)s()

    59

{

    60

    __FirstTick = 1;

    61

%(init_entry_variables)s

    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

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

    77

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

    78

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

    79

%(init_axis_params)s

    80

%(fieldbus_interface_definition)s

    81

		__FirstTick = 0;

    82

	}

    83

    84

	// CIA402 node state computation

    85

	__CIA402Node_%(location)s.state = __Unknown;

    86

	switch (statusword_inactive) {

    87

		case 0x00:

    88

			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;

    89

			break;

    90

		case 0x40:

    91

			__CIA402Node_%(location)s.state = __SwitchOnDisabled;

    92

			break;

    93

		case 0x0f:

    94

			__CIA402Node_%(location)s.state = __FaultReactionActive;

    95

			break;

    96

		case 0x08:

    97

			__CIA402Node_%(location)s.state = __Fault;

    98

			break;

    99

		default:

   100

			break;

   101

	}

   102

	switch (statusword_active) {

   103

		case 0x21:

   104

			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;

   105

			break;

   106

		case 0x23:

   107

			__CIA402Node_%(location)s.state = __SwitchedOn;

   108

			break;

   109

		case 0x27:

   110

			__CIA402Node_%(location)s.state = __OperationEnabled;

   111

			break;

   112

		case 0x07:

   113

			__CIA402Node_%(location)s.state = __QuickStopActive;

   114

			break;

   115

		default:

   116

			break;

   117

	}

   118

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

   119

		return;

   120

	}

   121

   122

	// Default variables retrieve

   123

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

   124

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

   125

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

   126

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

   127

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

   128

	__CIA402Node_%(location)s.axis->ActualTorque = TORQUE_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualTorque));

   129

   130

	// Extra variables retrieve

   131

%(extra_variables_retrieve)s

   132

}

   133

   134

void __publish_%(location)s()

   135

{

   136

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

   137

   138

	// CIA402 node state transition computation

   139

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

   140

	    case __SwitchOnDisabled:

   141

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

   142

	    	break;

   143

	    case __ReadyToSwitchOn:

   144

	    case __OperationEnabled:

   145

	    	if (!power) {

   146

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

   147

	    		break;

   148

	    	}

   149

	    case __SwitchedOn:

   150

	    	if (power) {

   151

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

   152

	    	}

   153

	    	break;

   154

	    case __Fault:

   155

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

   156

	    	break;

   157

	    default:

   158

	    	break;

   159

	}

   160

   161

	// CIA402 node modes of operation computation according to axis motion mode

   162

	switch (__CIA402Node_%(location)s.axis->AxisMotionMode) {

   163

		case mc_mode_cst:

   164

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

   165

			break;

   166

		case mc_mode_csv:

   167

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

   168

			break;

   169

		default:

   170

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

   171

			break;

   172

	}

   173

   174

	// Default variables publish

   175

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

   176

	*(__CIA402Node_%(location)s.TargetVelocity) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->VelocitySetPoint);

   177

	*(__CIA402Node_%(location)s.TargetTorque) = TORQUE_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->TorqueSetPoint);

   178

   179

	// Extra variables publish

   180

%(extra_variables_publish)s

   181

}