1

/*

     2

     3

Template C code used to produce target Ethercat C CIA402 code

     4

     5

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

     8

     9

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

    10

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

    11

(at your option) any later version.

    12

    13

See COPYING file for copyrights details.

    14

    15

*/

    16

    17

#include "ecrt.h"

    18

    19

#include "beremiz.h"

    20

#include "iec_types_all.h"

    21

    22

#include "accessor.h"

    23

#include "POUS.h"

    24

    25

/* From CiA402, page 27

    26

    27

        Table 30 - State coding

    28

    Statusword      |      PDS FSA state

    29

xxxx xxxx x0xx 0000 | Not ready to switch on

    30

xxxx xxxx x1xx 0000 | Switch on disabled

    31

xxxx xxxx x01x 0001 | Ready to switch on

    32

xxxx xxxx x01x 0011 | Switched on

    33

xxxx xxxx x01x 0111 | Operation enabled

    34

xxxx xxxx x00x 0111 | Quick stop active

    35

xxxx xxxx x0xx 1111 | Fault reaction active

    36

xxxx xxxx x0xx 1000 | Fault

    37

*/

    53

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

    57

#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000

    58

#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000

    59

#define ReadyToSwitchOn     0b00100001

    60

#define SwitchedOn          0b00100011

    61

#define OperationEnabled    0b00100111

    62

#define QuickStopActive     0b00000111

    63

#define FaultReactionActive 0b00001111 FSA_sep 0b00101111

    64

#define Fault               0b00001000 FSA_sep 0b00101000

    65

    76

#define SW_ReadyToSwitchOn     0x0001

    77

#define SW_SwitchedOn          0x0002

    78

#define SW_OperationEnabled    0x0004

    79

#define SW_Fault               0x0008

    80

#define SW_VoltageEnabled      0x0010

    81

#define SW_QuickStop           0x0020

    82

#define SW_SwitchOnDisabled    0x0040

    83

#define SW_Warning             0x0080

    84

#define SW_Remote              0x0200

    85

#define SW_TargetReached       0x0400

    86

#define SW_InternalLimitActive 0x0800

    87

    92

// ControlWord bits :

    93

#define SwitchOn        0x0001

    94

#define EnableVoltage   0x0002

    95

#define QuickStop       0x0004

    96

#define EnableOperation 0x0008

    97

#define FaultReset      0x0080

    98

#define Halt            0x0100

    99

   109

   110

%(MCL_headers)s

   111

   112

static IEC_BOOL __FirstTick = 1;

   113

   114

typedef struct {

   115

%(entry_variables)s

   116

    axis_s* axis;

   117

} __CIA402Node;

   118

   120

   121

static __CIA402Node AxsPub;

   122

   123

%(extern_located_variables_declaration)s

   124

   125

%(fieldbus_interface_declaration)s

   126

   128

{

   129

    __FirstTick = 1;

   130

%(init_entry_variables)s

   131

    return 0;

   132

}

   133

   139

{

   140

	if (__FirstTick) {

   142

		AxsPub.axis = 

   145

%(init_axis_params)s

   146

%(fieldbus_interface_definition)s

   147

		__FirstTick = 0;

   148

	}

   149

   150

	// Default variables retrieve

   151

	AxsPub.axis->CommunicationReady = 

   152

        *(AxsPub.StatusWord) != 0;

   153

#define FSA_sep || FSA ==

   154

    {

   155

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

   156

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

   157

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

   158

    }

   159

#undef FSA_sep 

   161

   162

	// Extra variables retrieve

   163

%(extra_variables_retrieve)s

   164

}

   165

   167

{

   168

	IEC_BOOL power = 

   169

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

   170

        && AxsPub.axis->Power;

   171

    uint16_t CW = *(AxsPub.ControlWord);

   172

   173

#define FSA_sep : case

   174

	// CIA402 node state transition computation

   175

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

   176

	    case SwitchOnDisabled :

   177

            CW &= ~(SwitchOn | FaultReset);

   178

            CW |= EnableVoltage | QuickStop;

   179

	    	break;

   180

	    case ReadyToSwitchOn :

   181

	    case OperationEnabled :

   182

	    	if (!power) {

   183

                CW &= ~(FaultReset | EnableOperation);

   184

                CW |= SwitchOn | EnableVoltage | QuickStop;

   185

	    		break;

   186

	    	}

   187

	    case SwitchedOn :

   188

	    	if (power) {

   189

                CW &= ~(FaultReset);

   190

                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

   191

	    	}

   192

	    	break;

   200

	    default:

   201

	    	break;

   202

	}

   235

#undef FSA_sep 

   242

   243

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

   245

   246

	// Default variables publish

   248

   249

	// Extra variables publish

   250

%(extra_variables_publish)s

   251

}