HGKeeper

Template C code used to produce target Ethercat C CIA402 code

RTES Lab : CRKim, JBLee, youcu

Higen Motor : Donggu Kang

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

//ssh_add

/* From CiA402, Page 63 Statusword for homing mode

Table 106 - Definition of bit 10, bit 12, bit 13

xx00 x0xx xxxx xxxx | Homing procedure is in progress

xx00 x1xx xxxx xxxx | Homing procedure is interrupted or not started

xx01 x0xx xxxx xxxx | Homing is attained, but target is not reached

xx01 x1xx xxxx xxxx | Homing procedure is completed successfully

xx10 x0xx xxxx xxxx | Homing error occurred, velocity is not 0

xx10 x1xx xxxx xxxx | Homing error occurred, velocity is 0

xx11 xxxx xxxx xxxx | reserved

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

//ssh_add

#define HomingStatusWord(SW) (SW & 0x3400)

#define FaultFromStatusWord(SW) (SW & 0x0008)

#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

//ssh_add

#define HomingInProgress 0b0000000000000000

#define HomingNotRunning 0b0000010000000000

#define HomingNotReached 0b0001000000000000

#define Homing_Completed 0b0001010000000000

#define HomingErrorInVelo 0b0010000000000000

#define HomingErrorNotVelo 0b0010010000000000

#define HomingReserved 0b0011000000000000 FSA_sep 0b0011010000000000

// StatusWord 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

//ssh_add

#define SW_HomingAttained 0x1000

#define SW_HomingError 0x2000

// ControlWord bits :

#define SwitchOn 0x0001

#define EnableVoltage 0x0002

#define QuickStop 0x0004

#define EnableOperation 0x0008

#define FaultReset 0x0080

#define Halt 0x0100

//ssh_add

//#define Homing_OperationStart 0x0010

#define Homing_OperationStart_Origin 0x0010

#define Homing_OperationStart_Edit 0x001F

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

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

IEC_INT beremiz__IW%(location)s_402;

IEC_INT *__IW%(location)s_402 = &beremiz__IW%(location)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)s

static __CIA402Node AxsPub;

%(extern_located_variables_declaration)s

%(fieldbus_interface_declaration)s

int __init_%(location)s()

{

__FirstTick = 1;

%(init_entry_variables)s

return 0;

}

void __cleanup_%(location)s()

{

}

void __retrieve_%(location)s()

{

if (__FirstTick) {

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

AxsPub.axis =

__MK_GetPublic_AXIS_REF(*__IW%(location)s_402);

AxsPub.axis->NetworkPosition = beremiz__IW%(location)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

%(default_variables_retrieve)s

// Extra variables retrieve

%(extra_variables_retrieve)s

}

void __publish_%(location)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;

//ssh_check

// case Fault :

// /* TODO reset fault only when MC_Reset */

// AxsPub.axis->DriveFault = 1;

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

// CW |= FaultReset;

// break;

default:

break;

}

//ssh_add

if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault)

AxsPub.axis->DriveFault = 1;

else{

AxsPub.axis->DriveFault = 0;

AxsPub.axis->DriveFaultReset = 0;

}

if(AxsPub.axis->DriveFaultReset){

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

CW |= FaultReset;

}

//ssh_add

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

case HomingInProgress:

break;

case HomingNotRunning:

break;

case HomingNotReached:

break;

case Homing_Completed:

if(!AxsPub.axis->HomingCompleted)

AxsPub.axis->HomingCompleted = 1;

break;

case HomingErrorInVelo:

case HomingErrorNotVelo:

if(!AxsPub.axis->HomingCompleted)

AxsPub.axis->HomingCompleted = 1;

break;

case HomingReserved:

break;

}

#undef FSA_sep

//ssh_add

%(modeofop_homing_method)s

*(AxsPub.ControlWord) = CW;

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

%(modeofop_computation_mode)s

// Default variables publish

%(default_variables_publish)s

// Extra variables publish

%(extra_variables_publish)s

}