/*
Template C code used to produce target Ethercat C CIA402 code
Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
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
}