1

/*

    14

    15

#include "ecrt.h"

    16

    17

#include "beremiz.h"

    18

#include "iec_types_all.h"

    19

    20

#include "accessor.h"

    21

#include "POUS.h"

    22

    23

/* From CiA402, page 27

    24

    25

        Table 30 - State coding

    26

    Statusword      |      PDS FSA state

    27

xxxx xxxx x0xx 0000 | Not ready to switch on

    28

xxxx xxxx x1xx 0000 | Switch on disabled

    29

xxxx xxxx x01x 0001 | Ready to switch on

    30

xxxx xxxx x01x 0011 | Switched on

    31

xxxx xxxx x01x 0111 | Operation enabled

    32

xxxx xxxx x00x 0111 | Quick stop active

    33

xxxx xxxx x0xx 1111 | Fault reaction active

    34

xxxx xxxx x0xx 1000 | Fault

    35

*/

    36

#define FSAFromStatusWord(SW) (SW & 0x006f)

    37

#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000

    38

#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000

    39

#define ReadyToSwitchOn     0b00100001

    40

#define SwitchedOn          0b00100011

    41

#define OperationEnabled    0b00100111

    42

#define QuickStopActive     0b00000111

    43

#define FaultReactionActive 0b00001111 FSA_sep 0b00101111

    44

#define Fault               0b00001000 FSA_sep 0b00101000

    45

    46

// SatusWord bits :

    47

#define SW_ReadyToSwitchOn     0x0001

    48

#define SW_SwitchedOn          0x0002

    49

#define SW_OperationEnabled    0x0004

    50

#define SW_Fault               0x0008

    51

#define SW_VoltageEnabled      0x0010

    52

#define SW_QuickStop           0x0020

    53

#define SW_SwitchOnDisabled    0x0040

    54

#define SW_Warning             0x0080

    55

#define SW_Remote              0x0200

    56

#define SW_TargetReached       0x0400

    57

#define SW_InternalLimitActive 0x0800

    58

    59

// ControlWord bits :

    60

#define SwitchOn        0x0001

    61

#define EnableVoltage   0x0002

    62

#define QuickStop       0x0004

    63

#define EnableOperation 0x0008

    64

#define FaultReset      0x0080

    65

#define Halt            0x0100

    66

    67

    68

IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s;

    69

IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s;

    70

IEC_INT beremiz__IW%(location_str)s_402;

    71

IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402;

    72

    73

%(MCL_headers)s

    74

    75

static IEC_BOOL __FirstTick = 1;

    76

    77

typedef struct {

    78

%(entry_variables)s

    79

    axis_s* axis;

    80

} __CIA402Node;

    81

    82

#define AxsPub __CIA402Node_%(location_str)s

    83

    84

static __CIA402Node AxsPub;

    85

    86

%(extern_located_variables_declaration)s

    87

    88

%(fieldbus_interface_declaration)s

    89

    90

int __init_%(location_str)s()

    91

{

    92

    __FirstTick = 1;

    93

%(init_entry_variables)s

    94

	*(AxsPub.ModesOfOperation) = 0x08;

    95

    return 0;

    96

}

    97

    98

void __cleanup_%(location_str)s()

    99

{

   100

}

   101

   102

void __retrieve_%(location_str)s()

   103

{

   104

	if (__FirstTick) {

   105

		*__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();

   106

		AxsPub.axis = 

   107

            __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);

   108

		AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s;

   109

%(init_axis_params)s

   110

%(fieldbus_interface_definition)s

   111

		__FirstTick = 0;

   112

	}

   113

   114

	// Default variables retrieve

   115

	AxsPub.axis->CommunicationReady = 

   116

        *(AxsPub.StatusWord) != 0;

   117

#define FSA_sep || FSA ==

   118

    {

   119

        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));

   120

        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;

   121

        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;

   122

    }

   123

#undef FSA_sep 

   124

	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);

   125

	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);

   126

	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);

   127

   128

	// Extra variables retrieve

   129

%(extra_variables_retrieve)s

   130

}

   131

   132

void __publish_%(location_str)s()

   133

{

   134

	IEC_BOOL power = 

   135

        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 

   136

        && AxsPub.axis->Power;

   137

    uint16_t CW = *(AxsPub.ControlWord);

   138

   139

#define FSA_sep : case

   140

	// CIA402 node state transition computation

   141

	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {

   142

	    case SwitchOnDisabled :

   143

            CW &= ~(SwitchOn | FaultReset);

   144

            CW |= EnableVoltage | QuickStop;

   145

	    	break;

   146

	    case ReadyToSwitchOn :

   147

	    case OperationEnabled :

   148

	    	if (!power) {

   149

                CW &= ~(FaultReset | EnableOperation);

   150

                CW |= SwitchOn | EnableVoltage | QuickStop;

   151

	    		break;

   152

	    	}

   153

	    case SwitchedOn :

   154

	    	if (power) {

   155

                CW &= ~(FaultReset);

   156

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   157

	    	}

   158

	    	break;

   159

	    case Fault :

   160

            /* TODO reset fault only when MC_Reset */

   161

            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);

   162

            CW |= FaultReset;

   163

	    	break;

   164

	    default:

   165

	    	break;

   166

	}

   167

#undef FSA_sep 

   168

    *(AxsPub.ControlWord) = CW;

   169

   170

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

   171

	switch (AxsPub.axis->AxisMotionMode) {

   172

		case mc_mode_cst:

   173

			*(AxsPub.ModesOfOperation) = 0x0a;

   174

			break;

   175

		case mc_mode_csv:

   176

			*(AxsPub.ModesOfOperation) = 0x09;

   177

			break;

   178

		default:

   179

			*(AxsPub.ModesOfOperation) = 0x08;

   180

			break;

   181

	}

   182

   183

	// Default variables publish

   184

	*(AxsPub.TargetPosition) = 

   185

            AxsPub.axis->RawPositionSetPoint;

   186

	*(AxsPub.TargetVelocity) = 

   187

            AxsPub.axis->RawVelocitySetPoint;

   188

	*(AxsPub.TargetTorque) = 

   189

            AxsPub.axis->RawTorqueSetPoint;

   190

   191

	// Extra variables publish

   192

%(extra_variables_publish)s

   193

}