1 /*

     2  * Ethercat DS402 node execution code

     3  *

     4  * */

     5 

     6 #include "ecrt.h"

     7 

     8 #ifdef _WINDOWS_H

     9   #include "iec_types.h"

    10 #else

    11   #include "iec_std_lib.h"

    12 #endif

    13 

    14 IEC_INT beremiz__IW%(location)s_0;

    15 IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0;

    16 

    17 %(MCL_headers)s

    18 

    19 IEC_UINT __InactiveMask = 0x4f;

    20 IEC_UINT __ActiveMask = 0x6f;

    21 IEC_UINT __PowerMask = 0x10;

    22 IEC_BOOL __FirstTick = 1;

    23 

    24 typedef enum {

    25 	__Unknown,

    26 	__NotReadyToSwitchOn,

    27 	__SwitchOnDisabled,

    28 	__ReadyToSwitchOn,

    29 	__SwitchedOn,

    30 	__OperationEnabled,

    31 	__QuickStopActive,

    32 	__FaultReactionActive,

    33 	__Fault,

    34 } __DS402NodeState;

    35 

    36 typedef struct {

    37 %(entry_variables)s

    38 	__DS402NodeState state;

    39 	axis_s* axis;

    40 } __DS402Node;

    41 

    42 static __DS402Node __DS402Node_%(location)s;

    43 

    44 %(extern_located_variables_declaration)s

    45 

    46 int __init_%(location)s()

    47 {

    48 %(init_entry_variables)s

    49 	*__IW%(location)s_0 = __MK_AllocAxis(&(__DS402Node_%(location)s.axis));

    50 	return 0;

    51 }

    52 

    53 void __cleanup_%(location)s()

    54 {

    55 }

    56 

    57 void __retrieve_%(location)s()

    58 {

    59 	IEC_UINT statusword_inactive = *(__DS402Node_%(location)s.StatusWord) & __InactiveMask;

    60 	IEC_UINT statusword_active = *(__DS402Node_%(location)s.StatusWord) & __ActiveMask;

    61 

    62 	if (__FirstTick) {

    63 %(init_axis_params)s

    64 		_FirstTick = 0;

    65 	}

    66 

    67 	// DS402 node state computation

    68 	__DS402Node_%(location)s.state = __Unknown;

    69 	switch (statusword_inactive) {

    70 		case 0x00:

    71 			__DS402Node_%(location)s.state = __NotReadyToSwitchOn;

    72 			break;

    73 		case 0x40:

    74 			__DS402Node_%(location)s.state = __SwitchOnDisabled;

    75 			break;

    76 		case 0x0f:

    77 			__DS402Node_%(location)s.state = __FaultReactionActive;

    78 			break;

    79 		case 0x08:

    80 			__DS402Node_%(location)s.state = __Fault;

    81 			break;

    82 		default:

    83 			break;

    84 	}

    85 	switch (statusword_active) {

    86 		case 0x21:

    87 			__DS402Node_%(location)s.state = __ReadyToSwitchOn;

    88 			break;

    89 		case 0x23:

    90 			__DS402Node_%(location)s.state = __SwitchedOn;

    91 			break;

    92 		case 0x27:

    93 			__DS402Node_%(location)s.state = __OperationEnabled;

    94 			break;

    95 		case 0x07:

    96 			__DS402Node_%(location)s.state = __QuickStopActive;

    97 			break;

    98 		default:

    99 			break;

   100 	}

   101 	if (__DS402Node_%(location)s.state == __Unknown) {

   102 		return;

   103 	}

   104 

   105 	__DS402Node_%(location)s.axis->PowerFeedback = __DS402Node_%(location)s.state == __OperationEnabled;

   106 	__DS402Node_%(location)s.axis->ActualPosition = (IEC_REAL)(*(__DS402Node_%(location)s.ActualPosition)) * __DS402Node_%(location)s.axis->RatioDenominator / __DS402Node_%(location)s.axis->RatioNumerator;

   107 

   108 	__MK_UpdateAxis(*__IW%(location)s_0);

   109 }

   110 

   111 void __publish_%(location)s()

   112 {

   113 	__MK_ComputeAxis(*__IW%(location)s_0);

   114 

   115 	IEC_BOOL power = ((*(__DS402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __DS402Node_%(location)s.axis->Power;

   116 

   117 	// DS402 node state transition computation

   118 	switch (__DS402Node_%(location)s.state) {

   119 	    case __SwitchOnDisabled:

   120 	    	*(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x87) | 0x06;

   121 	    	break;

   122 	    case __ReadyToSwitchOn:

   123 	    case __OperationEnabled:

   124 	    	if (!power) {

   125 	    		*(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x07;

   126 	    		break;

   127 	    	}

   128 	    case __SwitchedOn:

   129 	    	if (power) {

   130 	    	    *(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f;

   131 	    	}

   132 	    	break;

   133 	    case __Fault:

   134 	    	*(__DS402Node_%(location)s.ControlWord) = (*(__DS402Node_%(location)s.ControlWord) & ~0x8f) | 0x80;

   135 	    	break;

   136 	    default:

   137 	    	break;

   138 	}

   139 

   140 	if (__DS402Node_%(location)s.axis->CSP && *(__DS402Node_%(location)s.ModesOfOperationDisplay) == 0x08) {

   141 		*(__DS402Node_%(location)s.TargetPosition) = (IEC_DINT)(__DS402Node_%(location)s.axis->PositionSetPoint * __DS402Node_%(location)s.axis->RatioNumerator / __DS402Node_%(location)s.axis->RatioDenominator);

   142 	}

   143 	else {

   144 		*(__DS402Node_%(location)s.TargetPosition) = *(__DS402Node_%(location)s.ActualPosition);

   145 	}

   146 }