diff -r 09d5d1456616 -r c9deff128c37 etherlab/plc_cia402node.c --- a/etherlab/plc_cia402node.c Sat Jun 23 09:17:20 2018 +0200 +++ b/etherlab/plc_cia402node.c Wed Nov 20 16:57:15 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 @@ -79,7 +116,7 @@ axis_s* axis; } __CIA402Node; -#define AxsPub __CIA402Node_%(location_str)s +#define AxsPub __CIA402Node_%(location)s static __CIA402Node AxsPub; @@ -87,25 +124,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; @@ -121,15 +157,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) @@ -156,37 +190,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