--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/etherlab/plc_cia402node.c Sat Jun 23 09:08:13 2018 +0200
@@ -0,0 +1,193 @@
+/*
+
+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 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
+}