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@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@2150: #define FSAFromStatusWord(SW) (SW & 0x006f)
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@2150: // SatusWord 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@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@2150: 
Laurent@2154: IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s;
Laurent@2154: IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s;
Laurent@2154: IEC_INT beremiz__IW%(location_str)s_402;
Laurent@2154: IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)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: 
Laurent@2153: #define AxsPub __CIA402Node_%(location_str)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: 
Laurent@2153: int __init_%(location_str)s()
laurent@2032: {
Laurent@2118:     __FirstTick = 1;
laurent@2041: %(init_entry_variables)s
Edouard@2150: 	*(AxsPub.ModesOfOperation) = 0x08;
Laurent@2079:     return 0;
laurent@2032: }
laurent@2032: 
Laurent@2153: void __cleanup_%(location_str)s()
laurent@2032: {
laurent@2032: }
laurent@2032: 
Laurent@2153: void __retrieve_%(location_str)s()
laurent@2032: {
laurent@2041: 	if (__FirstTick) {
Laurent@2154: 		*__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();
Edouard@2150: 		AxsPub.axis = 
Laurent@2154:             __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);
Laurent@2154: 		AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)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@2150: 	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);
Edouard@2150: 	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);
Edouard@2150: 	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);
Laurent@2084: 
Laurent@2084: 	// Extra variables retrieve
Laurent@2084: %(extra_variables_retrieve)s
laurent@2032: }
laurent@2032: 
Laurent@2153: void __publish_%(location_str)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@2150: 	    case Fault :
Edouard@2150:             /* TODO reset fault only when MC_Reset */
Edouard@2150:             CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
Edouard@2150:             CW |= FaultReset;
laurent@2032: 	    	break;
laurent@2032: 	    default:
laurent@2032: 	    	break;
laurent@2032: 	}
Edouard@2150: #undef FSA_sep 
Edouard@2150:     *(AxsPub.ControlWord) = CW;
laurent@2032: 
Laurent@2136: 	// CIA402 node modes of operation computation according to axis motion mode
Edouard@2150: 	switch (AxsPub.axis->AxisMotionMode) {
Laurent@2136: 		case mc_mode_cst:
Edouard@2150: 			*(AxsPub.ModesOfOperation) = 0x0a;
Laurent@2136: 			break;
Laurent@2136: 		case mc_mode_csv:
Edouard@2150: 			*(AxsPub.ModesOfOperation) = 0x09;
Laurent@2136: 			break;
Laurent@2136: 		default:
Edouard@2150: 			*(AxsPub.ModesOfOperation) = 0x08;
Laurent@2136: 			break;
Laurent@2136: 	}
Laurent@2136: 
Laurent@2084: 	// Default variables publish
Edouard@2150: 	*(AxsPub.TargetPosition) = 
Edouard@2150:             AxsPub.axis->RawPositionSetPoint;
Edouard@2150: 	*(AxsPub.TargetVelocity) = 
Edouard@2150:             AxsPub.axis->RawVelocitySetPoint;
Edouard@2150: 	*(AxsPub.TargetTorque) = 
Edouard@2150:             AxsPub.axis->RawTorqueSetPoint;
Laurent@2084: 
Laurent@2084: 	// Extra variables publish
Laurent@2084: %(extra_variables_publish)s
laurent@2032: }