--- a/etherlab/plc_cia402node.c Wed Nov 13 11:21:04 2019 +0100
+++ b/etherlab/plc_cia402node.c Mon Nov 25 08:10:45 2019 +0100
@@ -3,6 +3,8 @@
Template C code used to produce target Ethercat C CIA402 code
Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
+ 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
@@ -33,7 +35,25 @@
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
@@ -43,7 +63,16 @@
#define FaultReactionActive 0b00001111 FSA_sep 0b00101111
#define Fault 0b00001000 FSA_sep 0b00101000
-// SatusWord bits :
+//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
@@ -56,6 +85,10 @@
#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
@@ -64,11 +97,15 @@
#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;
+//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
@@ -104,7 +141,7 @@
axis_s* axis;
} __CIA402Node;
-#define AxsPub __CIA402Node_%(location_str)s
+#define AxsPub __CIA402Node_%(location)s
static __CIA402Node AxsPub;
@@ -112,25 +149,24 @@
%(fieldbus_interface_declaration)s
-int __init_%(location_str)s()
+int __init_%(location)s()
{
__FirstTick = 1;
%(init_entry_variables)s
- *(AxsPub.ModesOfOperation) = 0x08;
return 0;
}
-void __cleanup_%(location_str)s()
-{
-}
-
-void __retrieve_%(location_str)s()
+void __cleanup_%(location)s()
+{
+}
+
+void __retrieve_%(location)s()
{
if (__FirstTick) {
- *__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();
+ *__IW%(location)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;
+ __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;
@@ -146,15 +182,13 @@
AxsPub.axis->PowerFeedback = FSA == OperationEnabled;
}
#undef FSA_sep
- AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);
- AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);
- AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);
+%(default_variables_retrieve)s
// Extra variables retrieve
%(extra_variables_retrieve)s
}
-void __publish_%(location_str)s()
+void __publish_%(location)s()
{
IEC_BOOL power =
((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0)
@@ -181,37 +215,61 @@
CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
}
break;
- case Fault :
- /* TODO reset fault only when MC_Reset */
- CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
- CW |= FaultReset;
- 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
- *(AxsPub.ControlWord) = CW;
+
+ //ssh_add
+%(modeofop_homing_method)s
+
+ *(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;
- }
+%(modeofop_computation_mode)s
// Default variables publish
- *(AxsPub.TargetPosition) =
- AxsPub.axis->RawPositionSetPoint;
- *(AxsPub.TargetVelocity) =
- AxsPub.axis->RawVelocitySetPoint;
- *(AxsPub.TargetTorque) =
- AxsPub.axis->RawTorqueSetPoint;
+%(default_variables_publish)s
// Extra variables publish
%(extra_variables_publish)s