HGKeeper

Template C code used to produce target Ethercat C CIA402 code

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

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

(at your option) any later version.

See COPYING file for copyrights details.

#include "ecrt.h"

#include "beremiz.h"

#include "iec_types_all.h"

#include "accessor.h"

#include "POUS.h"

/* From CiA402, page 27

Table 30 - State coding

Statusword | PDS FSA state

xxxx xxxx x0xx 0000 | Not ready to switch on

xxxx xxxx x1xx 0000 | Switch on disabled

xxxx xxxx x01x 0001 | Ready to switch on

xxxx xxxx x01x 0011 | Switched on

xxxx xxxx x01x 0111 | Operation enabled

xxxx xxxx x00x 0111 | Quick stop active

xxxx xxxx x0xx 1111 | Fault reaction active

xxxx xxxx x0xx 1000 | Fault

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

#define NotReadyToSwitchOn 0b00000000 FSA_sep 0b00100000

#define SwitchOnDisabled 0b01000000 FSA_sep 0b01100000

#define ReadyToSwitchOn 0b00100001

#define SwitchedOn 0b00100011

#define OperationEnabled 0b00100111

#define QuickStopActive 0b00000111

#define FaultReactionActive 0b00001111 FSA_sep 0b00101111

#define Fault 0b00001000 FSA_sep 0b00101000

// SatusWord bits :

#define SW_ReadyToSwitchOn 0x0001

#define SW_SwitchedOn 0x0002

#define SW_OperationEnabled 0x0004

#define SW_Fault 0x0008

#define SW_VoltageEnabled 0x0010

#define SW_QuickStop 0x0020

#define SW_SwitchOnDisabled 0x0040

#define SW_Warning 0x0080

#define SW_Remote 0x0200

#define SW_TargetReached 0x0400

#define SW_InternalLimitActive 0x0800

// ControlWord bits :

#define SwitchOn 0x0001

#define EnableVoltage 0x0002

#define QuickStop 0x0004

#define EnableOperation 0x0008

#define FaultReset 0x0080

#define Halt 0x0100

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

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

IEC_INT beremiz__IW%(location_str)s_402;

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

%(MCL_headers)s

static IEC_BOOL __FirstTick = 1;

typedef enum {

mc_mode_none, // No motion mode

mc_mode_csp, // Continuous Synchronous Positionning mode

mc_mode_csv, // Continuous Synchronous Velocity mode

mc_mode_cst, // Continuous Synchronous Torque mode

} mc_axismotionmode_enum;

typedef struct {

IEC_BOOL Power;

IEC_BOOL CommunicationReady;

IEC_UINT NetworkPosition;

IEC_BOOL ReadyForPowerOn;

IEC_BOOL PowerFeedback;

IEC_DINT ActualRawPosition;

IEC_DINT ActualRawVelocity;

IEC_DINT ActualRawTorque;

IEC_DINT RawPositionSetPoint;

IEC_DINT RawVelocitySetPoint;

IEC_DINT RawTorqueSetPoint;

mc_axismotionmode_enum AxisMotionMode;

IEC_LREAL ActualVelocity;

IEC_LREAL ActualPosition;

IEC_LREAL ActualTorque;

}axis_s;

typedef struct {

%(entry_variables)s

axis_s* axis;

} __CIA402Node;

#define AxsPub __CIA402Node_%(location_str)s

static __CIA402Node AxsPub;

%(extern_located_variables_declaration)s

%(fieldbus_interface_declaration)s

int __init_%(location_str)s()

{

__FirstTick = 1;

%(init_entry_variables)s

*(AxsPub.ModesOfOperation) = 0x08;

return 0;

}

void __cleanup_%(location_str)s()

{

}

void __retrieve_%(location_str)s()

{

if (__FirstTick) {

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

AxsPub.axis =

__MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);

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

%(init_axis_params)s

%(fieldbus_interface_definition)s

__FirstTick = 0;

}

// Default variables retrieve

AxsPub.axis->CommunicationReady =

*(AxsPub.StatusWord) != 0;

#define FSA_sep || FSA ==

{

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

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

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

}

#undef FSA_sep

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

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

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

// Extra variables retrieve

%(extra_variables_retrieve)s

}

void __publish_%(location_str)s()

{

IEC_BOOL power =

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

&& AxsPub.axis->Power;

uint16_t CW = *(AxsPub.ControlWord);

#define FSA_sep : case

// CIA402 node state transition computation

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

case SwitchOnDisabled :

CW &= ~(SwitchOn | FaultReset);

CW |= EnableVoltage | QuickStop;

break;

case ReadyToSwitchOn :

case OperationEnabled :

if (!power) {

CW &= ~(FaultReset | EnableOperation);

CW |= SwitchOn | EnableVoltage | QuickStop;

break;

}

case SwitchedOn :

if (power) {

CW &= ~(FaultReset);

CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;

}

break;

case Fault :

/* TODO reset fault only when MC_Reset */

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

CW |= FaultReset;

break;

default:

break;

}

#undef FSA_sep

*(AxsPub.ControlWord) = CW;

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

switch (AxsPub.axis->AxisMotionMode) {

case mc_mode_cst:

*(AxsPub.ModesOfOperation) = 0x0a;

break;

case mc_mode_csv:

*(AxsPub.ModesOfOperation) = 0x09;

break;

default:

*(AxsPub.ModesOfOperation) = 0x08;

break;

}

// Default variables publish

*(AxsPub.TargetPosition) =

AxsPub.axis->RawPositionSetPoint;

*(AxsPub.TargetVelocity) =

AxsPub.axis->RawVelocitySetPoint;

*(AxsPub.TargetTorque) =

AxsPub.axis->RawTorqueSetPoint;

// Extra variables publish

%(extra_variables_publish)s

}