1

/*

     2

     3

Template C code used to produce target Ethercat C CIA402 code

     4

     5

Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT

     6

     7

Distributed under the terms of the GNU Lesser General Public License as

     8

published by the Free Software Foundation; either version 2 of the License, or

     9

(at your option) any later version.

    10

    11

See COPYING file for copyrights details.

    12

    13

*/

    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

   102

typedef struct {

   103

%(entry_variables)s

   104

    axis_s* axis;

   105

} __CIA402Node;

   106

   107

#define AxsPub __CIA402Node_%(location_str)s

   108

   109

static __CIA402Node AxsPub;

   110

   111

%(extern_located_variables_declaration)s

   112

   113

%(fieldbus_interface_declaration)s

   114

   115

int __init_%(location_str)s()

   116

{

   117

    __FirstTick = 1;

   118

%(init_entry_variables)s

   119

	*(AxsPub.ModesOfOperation) = 0x08;

   120

    return 0;

   121

}

   122

   123

void __cleanup_%(location_str)s()

   124

{

   125

}

   126

   127

void __retrieve_%(location_str)s()

   128

{

   129

	if (__FirstTick) {

   130

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

   131

		AxsPub.axis = 

   132

            __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);

   133

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

   134

%(init_axis_params)s

   135

%(fieldbus_interface_definition)s

   136

		__FirstTick = 0;

   137

	}

   138

   139

	// Default variables retrieve

   140

	AxsPub.axis->CommunicationReady = 

   141

        *(AxsPub.StatusWord) != 0;

   142

#define FSA_sep || FSA ==

   143

    {

   144

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

   145

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

   146

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

   147

    }

   148

#undef FSA_sep 

   149

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

   150

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

   151

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

   152

   153

	// Extra variables retrieve

   154

%(extra_variables_retrieve)s

   155

}

   156

   157

void __publish_%(location_str)s()

   158

{

   159

	IEC_BOOL power = 

   160

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

   161

        && AxsPub.axis->Power;

   162

    uint16_t CW = *(AxsPub.ControlWord);

   163

   164

#define FSA_sep : case

   165

	// CIA402 node state transition computation

   166

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

   167

	    case SwitchOnDisabled :

   168

            CW &= ~(SwitchOn | FaultReset);

   169

            CW |= EnableVoltage | QuickStop;

   170

	    	break;

   171

	    case ReadyToSwitchOn :

   172

	    case OperationEnabled :

   173

	    	if (!power) {

   174

                CW &= ~(FaultReset | EnableOperation);

   175

                CW |= SwitchOn | EnableVoltage | QuickStop;

   176

	    		break;

   177

	    	}

   178

	    case SwitchedOn :

   179

	    	if (power) {

   180

                CW &= ~(FaultReset);

   181

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   182

	    	}

   183

	    	break;

   184

	    case Fault :

   185

            /* TODO reset fault only when MC_Reset */

   186

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

   187

            CW |= FaultReset;

   188

	    	break;

   189

	    default:

   190

	    	break;

   191

	}

   192

#undef FSA_sep 

   193

    *(AxsPub.ControlWord) = CW;

   194

   195

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

   196

	switch (AxsPub.axis->AxisMotionMode) {

   197

		case mc_mode_cst:

   198

			*(AxsPub.ModesOfOperation) = 0x0a;

   199

			break;

   200

		case mc_mode_csv:

   201

			*(AxsPub.ModesOfOperation) = 0x09;

   202

			break;

   203

		default:

   204

			*(AxsPub.ModesOfOperation) = 0x08;

   205

			break;

   206

	}

   207

   208

	// Default variables publish

   209

	*(AxsPub.TargetPosition) = 

   210

            AxsPub.axis->RawPositionSetPoint;

   211

	*(AxsPub.TargetVelocity) = 

   212

            AxsPub.axis->RawVelocitySetPoint;

   213

	*(AxsPub.TargetTorque) = 

   214

            AxsPub.axis->RawTorqueSetPoint;

   215

   216

	// Extra variables publish

   217

%(extra_variables_publish)s

   218

}