laurent@2032: /*
edouard@2165: 
edouard@2165: Template C code used to produce target Ethercat C CIA402 code
edouard@2165: 
edouard@2165: Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
edouard@2641:                          RTES Lab : CRKim, JBLee, youcu
edouard@2641:                          Higen Motor : Donggu Kang
edouard@2165: 
edouard@2165: Distributed under the terms of the GNU Lesser General Public License as
edouard@2165: published by the Free Software Foundation; either version 2 of the License, or
edouard@2165: (at your option) any later version.
edouard@2165: 
edouard@2165: See COPYING file for copyrights details.
edouard@2165: 
edouard@2165: */
laurent@2032: 
laurent@2032: #include "ecrt.h"
laurent@2032: 
Laurent@2122: #include "beremiz.h"
Laurent@2122: #include "iec_types_all.h"
laurent@2032: 
Laurent@2094: #include "accessor.h"
Laurent@2092: #include "POUS.h"
Laurent@2092: 
Edouard@2150: /* From CiA402, page 27
Edouard@2150: 
Edouard@2150:         Table 30 - State coding
Edouard@2150:     Statusword      |      PDS FSA state
Edouard@2150: xxxx xxxx x0xx 0000 | Not ready to switch on
Edouard@2150: xxxx xxxx x1xx 0000 | Switch on disabled
Edouard@2150: xxxx xxxx x01x 0001 | Ready to switch on
Edouard@2150: xxxx xxxx x01x 0011 | Switched on
Edouard@2150: xxxx xxxx x01x 0111 | Operation enabled
Edouard@2150: xxxx xxxx x00x 0111 | Quick stop active
Edouard@2150: xxxx xxxx x0xx 1111 | Fault reaction active
Edouard@2150: xxxx xxxx x0xx 1000 | Fault
Edouard@2150: */
edouard@2641: 
edouard@2641: //ssh_add
edouard@2641: /* From CiA402, Page 63 Statusword for homing mode
edouard@2641: 
edouard@2641: 		Table 106 - Definition of bit 10, bit 12, bit 13
edouard@2641: 
edouard@2641: xx00 x0xx xxxx xxxx | Homing procedure is in progress
edouard@2641: xx00 x1xx xxxx xxxx | Homing procedure is interrupted or not started
edouard@2641: xx01 x0xx xxxx xxxx | Homing is attained, but target is not reached
edouard@2641: xx01 x1xx xxxx xxxx | Homing procedure is completed successfully
edouard@2641: xx10 x0xx xxxx xxxx | Homing error occurred, velocity is not 0
edouard@2641: xx10 x1xx xxxx xxxx | Homing error occurred, velocity is 0
edouard@2641: xx11 xxxx xxxx xxxx | reserved
edouard@2641: */
edouard@2641: 
Edouard@2150: #define FSAFromStatusWord(SW) (SW & 0x006f)
edouard@2641: //ssh_add
edouard@2641: #define HomingStatusWord(SW) (SW & 0x3400)
edouard@2641: #define FaultFromStatusWord(SW) (SW & 0x0008)
Edouard@2150: #define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000
Edouard@2150: #define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000
Edouard@2150: #define ReadyToSwitchOn     0b00100001
Edouard@2150: #define SwitchedOn          0b00100011
Edouard@2150: #define OperationEnabled    0b00100111
Edouard@2150: #define QuickStopActive     0b00000111
Edouard@2150: #define FaultReactionActive 0b00001111 FSA_sep 0b00101111
Edouard@2150: #define Fault               0b00001000 FSA_sep 0b00101000
Edouard@2150: 
edouard@2641: //ssh_add
edouard@2641: #define HomingInProgress	0b0000000000000000
edouard@2641: #define HomingNotRunning	0b0000010000000000
edouard@2641: #define HomingNotReached	0b0001000000000000
edouard@2641: #define Homing_Completed	0b0001010000000000
edouard@2641: #define HomingErrorInVelo	0b0010000000000000
edouard@2641: #define HomingErrorNotVelo	0b0010010000000000
edouard@2641: #define HomingReserved		0b0011000000000000 FSA_sep 0b0011010000000000
edouard@2641: 
edouard@2641: // StatusWord bits :
Edouard@2150: #define SW_ReadyToSwitchOn     0x0001
Edouard@2150: #define SW_SwitchedOn          0x0002
Edouard@2150: #define SW_OperationEnabled    0x0004
Edouard@2150: #define SW_Fault               0x0008
Edouard@2150: #define SW_VoltageEnabled      0x0010
Edouard@2150: #define SW_QuickStop           0x0020
Edouard@2150: #define SW_SwitchOnDisabled    0x0040
Edouard@2150: #define SW_Warning             0x0080
Edouard@2150: #define SW_Remote              0x0200
Edouard@2150: #define SW_TargetReached       0x0400
Edouard@2150: #define SW_InternalLimitActive 0x0800
Edouard@2150: 
edouard@2641: //ssh_add
edouard@2641: #define SW_HomingAttained		0x1000
edouard@2641: #define SW_HomingError			0x2000
edouard@2641: 
Edouard@2150: // ControlWord bits :
Edouard@2150: #define SwitchOn        0x0001
Edouard@2150: #define EnableVoltage   0x0002
Edouard@2150: #define QuickStop       0x0004
Edouard@2150: #define EnableOperation 0x0008
Edouard@2150: #define FaultReset      0x0080
Edouard@2150: #define Halt            0x0100
Edouard@2150: 
edouard@2641: //ssh_add
edouard@2641: //#define Homing_OperationStart 0x0010
edouard@2641: #define Homing_OperationStart_Origin 0x0010
edouard@2641: #define Homing_OperationStart_Edit 0x001F
edouard@2641: 
edouard@2641: IEC_INT beremiz__IW%(location)s = %(slave_pos)s;
edouard@2641: IEC_INT *__IW%(location)s = &beremiz__IW%(location)s;
edouard@2641: IEC_INT beremiz__IW%(location)s_402;
edouard@2641: IEC_INT *__IW%(location)s_402 = &beremiz__IW%(location)s_402;
laurent@2034: 
laurent@2034: %(MCL_headers)s
laurent@2032: 
Edouard@2059: static IEC_BOOL __FirstTick = 1;
laurent@2032: 
edouard@2491: typedef enum {
edouard@2491:     mc_mode_none, // No motion mode
edouard@2491:     mc_mode_csp,  // Continuous Synchronous Positionning mode
edouard@2491:     mc_mode_csv,  // Continuous Synchronous Velocity mode
edouard@2491:     mc_mode_cst,  // Continuous Synchronous Torque mode
edouard@2491: } mc_axismotionmode_enum;
edouard@2491: 
edouard@2491: typedef struct {
edouard@2491:    IEC_BOOL Power;
edouard@2491:    IEC_BOOL CommunicationReady;
edouard@2491:    IEC_UINT NetworkPosition;
edouard@2491:    IEC_BOOL ReadyForPowerOn;
edouard@2491:    IEC_BOOL PowerFeedback;
edouard@2491:    IEC_DINT ActualRawPosition;
edouard@2491:    IEC_DINT ActualRawVelocity;
edouard@2491:    IEC_DINT ActualRawTorque;
edouard@2491:    IEC_DINT RawPositionSetPoint;
edouard@2491:    IEC_DINT RawVelocitySetPoint;
edouard@2491:    IEC_DINT RawTorqueSetPoint;
edouard@2491:    mc_axismotionmode_enum AxisMotionMode;
edouard@2491:    IEC_LREAL ActualVelocity;
edouard@2491:    IEC_LREAL ActualPosition;
edouard@2491:    IEC_LREAL ActualTorque;
edouard@2491: }axis_s;
edouard@2491: 
laurent@2032: typedef struct {
laurent@2032: %(entry_variables)s
Laurent@2077:     axis_s* axis;
laurent@2043: } __CIA402Node;
laurent@2032: 
edouard@2641: #define AxsPub __CIA402Node_%(location)s
Laurent@2136: 
Edouard@2150: static __CIA402Node AxsPub;
laurent@2032: 
laurent@2034: %(extern_located_variables_declaration)s
laurent@2032: 
Laurent@2092: %(fieldbus_interface_declaration)s
Laurent@2092: 
edouard@2641: int __init_%(location)s()
laurent@2032: {
Laurent@2118:     __FirstTick = 1;
laurent@2041: %(init_entry_variables)s
Laurent@2079:     return 0;
laurent@2032: }
laurent@2032: 
edouard@2641: void __cleanup_%(location)s()
edouard@2641: {
edouard@2641: }
edouard@2641: 
edouard@2641: void __retrieve_%(location)s()
laurent@2032: {
laurent@2041: 	if (__FirstTick) {
edouard@2641: 		*__IW%(location)s_402 = __MK_Alloc_AXIS_REF();
Edouard@2150: 		AxsPub.axis = 
edouard@2641:             __MK_GetPublic_AXIS_REF(*__IW%(location)s_402);
edouard@2641: 		AxsPub.axis->NetworkPosition = beremiz__IW%(location)s;
laurent@2041: %(init_axis_params)s
Laurent@2118: %(fieldbus_interface_definition)s
laurent@2045: 		__FirstTick = 0;
laurent@2041: 	}
laurent@2041: 
Laurent@2084: 	// Default variables retrieve
Edouard@2150: 	AxsPub.axis->CommunicationReady = 
Edouard@2150:         *(AxsPub.StatusWord) != 0;
Edouard@2150: #define FSA_sep || FSA ==
Edouard@2150:     {
Edouard@2150:         uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));
Edouard@2150:         AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;
Edouard@2150:         AxsPub.axis->PowerFeedback = FSA == OperationEnabled;
Edouard@2150:     }
Edouard@2150: #undef FSA_sep 
edouard@2641: %(default_variables_retrieve)s
Laurent@2084: 
Laurent@2084: 	// Extra variables retrieve
Laurent@2084: %(extra_variables_retrieve)s
laurent@2032: }
laurent@2032: 
edouard@2641: void __publish_%(location)s()
laurent@2032: {
Edouard@2150: 	IEC_BOOL power = 
Edouard@2150:         ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 
Edouard@2150:         && AxsPub.axis->Power;
Edouard@2150:     uint16_t CW = *(AxsPub.ControlWord);
laurent@2034: 
Edouard@2150: #define FSA_sep : case
laurent@2043: 	// CIA402 node state transition computation
Edouard@2150: 	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {
Edouard@2150: 	    case SwitchOnDisabled :
Edouard@2150:             CW &= ~(SwitchOn | FaultReset);
Edouard@2150:             CW |= EnableVoltage | QuickStop;
laurent@2032: 	    	break;
Edouard@2150: 	    case ReadyToSwitchOn :
Edouard@2150: 	    case OperationEnabled :
laurent@2034: 	    	if (!power) {
Edouard@2150:                 CW &= ~(FaultReset | EnableOperation);
Edouard@2150:                 CW |= SwitchOn | EnableVoltage | QuickStop;
laurent@2034: 	    		break;
laurent@2034: 	    	}
Edouard@2150: 	    case SwitchedOn :
laurent@2034: 	    	if (power) {
Edouard@2150:                 CW &= ~(FaultReset);
Edouard@2150:                 CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
laurent@2034: 	    	}
laurent@2032: 	    	break;
edouard@2641: 			//ssh_check
edouard@2641: //	    case Fault :
edouard@2641: //            /* TODO reset fault only when MC_Reset */
edouard@2641: //	    	AxsPub.axis->DriveFault = 1;
edouard@2641: //            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
edouard@2641: //            CW |= FaultReset;
edouard@2641: //	    	break;
laurent@2032: 	    default:
laurent@2032: 	    	break;
laurent@2032: 	}
edouard@2641: 	//ssh_add
edouard@2641: 	if(FaultFromStatusWord(*(AxsPub.StatusWord)) == SW_Fault)
edouard@2641: 		AxsPub.axis->DriveFault = 1;
edouard@2641: 	else{
edouard@2641: 		AxsPub.axis->DriveFault = 0;
edouard@2641: 		AxsPub.axis->DriveFaultReset = 0;
edouard@2641: 	}
edouard@2641: 	if(AxsPub.axis->DriveFaultReset){
edouard@2641: 		CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
edouard@2641: 		CW |= FaultReset;
edouard@2641: 	}
edouard@2641: 
edouard@2641: 	//ssh_add
edouard@2641: 	switch (HomingStatusWord(*(AxsPub.StatusWord))) {
edouard@2641: 		case HomingInProgress:
edouard@2641: 			break;
edouard@2641: 		case HomingNotRunning:
edouard@2641: 			break;
edouard@2641: 		case HomingNotReached:
edouard@2641: 			break;
edouard@2641: 		case Homing_Completed:
edouard@2641: 			if(!AxsPub.axis->HomingCompleted)
edouard@2641: 				AxsPub.axis->HomingCompleted = 1;
edouard@2641: 			break;
edouard@2641: 		case HomingErrorInVelo:
edouard@2641: 		case HomingErrorNotVelo:
edouard@2641: 			if(!AxsPub.axis->HomingCompleted)
edouard@2641: 				AxsPub.axis->HomingCompleted = 1;
edouard@2641: 			break;
edouard@2641: 		case HomingReserved:
edouard@2641: 			break;
edouard@2641: 	}
Edouard@2150: #undef FSA_sep 
edouard@2641: 
edouard@2641: 	//ssh_add
edouard@2641: %(modeofop_homing_method)s
edouard@2641: 
edouard@2641: 	*(AxsPub.ControlWord) = CW;
edouard@2641: 
laurent@2032: 
Laurent@2136: 	// CIA402 node modes of operation computation according to axis motion mode
edouard@2641: %(modeofop_computation_mode)s
Laurent@2136: 
Laurent@2084: 	// Default variables publish
edouard@2641: %(default_variables_publish)s
Laurent@2084: 
Laurent@2084: 	// Extra variables publish
Laurent@2084: %(extra_variables_publish)s
laurent@2032: }