//This file was generated by the LASAL2 CodeGenerator  -- 
//Please, do not edit this file (it might be overwritten by the next generator run)

//{{LSL_DEFINES
#define _DRIVEAXISDIAG_READPARAMETER            0
#define _DRIVEAXISDIAG_WRITEPARAMETER           1

#define _DRIVEAXISDIAG_ANGLE_MODE               6
#define _DRIVEAXISDIAG_CURRENT_SERVICEMODE      (-1)
#define _DRIVEAXISDIAG_ANGLE_SERVICEMODE        (-6)

#define _DRIVEAXISDIAG_ANGLE_STEPS_DEGREE       12    // value for move of one step as °
#define _DRIVEAXISDIAG_MAX_ANGLE_DEGREE         360   // degrees for on electrical turn
#define _DRIVEAXISDIAG_MAX_ANGLE_DEC            65535 // one electrical turn as decimal value 360° = 65535
#define _DRIVEAXISDIAG_ANGLE_STEPS_AMOUNT       (_DRIVEAXISDIAG_MAX_ANGLE_DEGREE/_DRIVEAXISDIAG_ANGLE_STEPS_DEGREE)   //amount of steps for one electrical turn 360°
#define _DRIVEAXISDIAG_ANGLE_ONESTEPS_DEC       (_DRIVEAXISDIAG_MAX_ANGLE_DEC/_DRIVEAXISDIAG_MAX_ANGLE_DEGREE)        // movement of 1° angle as decimal value (0...65535) 
#define _DRIVEAXISDIAG_ANGLE_STEPS_DEC          (_DRIVEAXISDIAG_MAX_ANGLE_DEC/_DRIVEAXISDIAG_ANGLE_STEPS_AMOUNT)      // movement of one angle step as decimal value (0...65535)

#define _DRIVEAXISDIAG_CURRENT_MAX_PERCENT      100  //Current steps from x% to 100%
#define _DRIVEAXISDIAG_CURRENTSTEP_PERCENT      10   //Current steps from x% to _DRIVEAXISDIAG_CURRENT_MAX_PERCENT%

#define _DRIVEAXISDIAG_MAX_MOTOR_POLES          256  //amount of maximal possible motor poles
#define _DRIVEAXISDIAG_MIN_MOTOR_POLES          2    //amount of minimal possible motor poles
#define _DRIVEAXISDIAG_FEEDBACK_MEASURE_POINTS  2    //amount of measure points for feedback offset. That two measuring points are sufficient was agreed with the Drive department
#define _DRIVEAXISDIAG_AXIS_MOTIONRUNS          1    //the axis is moved for feedback measure -> this value is how often the axis is turned to next position

#define _DRIVEAXISDIAG_ONE_TURN_HARDWARE        4294967295 //2^32
#define _DRIVEAXISDIAG_ONE_DEGREE_MECH          (_DRIVEAXISDIAG_ONE_TURN_HARDWARE/_DRIVEAXISDIAG_MAX_ANGLE_DEGREE)  // movement of 1° mechanical angle as decimal value (0...4294967296) 
#define _DRIVEAXISDIAG_STEP_DEGREE_MECH         (_DRIVEAXISDIAG_ONE_TURN_HARDWARE/_DRIVEAXISDIAG_ANGLE_STEPS_AMOUNT) // movement of axis mechanical divided by steps
#define _DRIVEAXISDIAG_WRONG_POSITION_RETRYS    2      //amount of allowed wrong positions before an error is set  

#define _DRIVEAXISDIAG_WAITTIME_GETIFPOS_START   1000  //waittime until first I-FPOS is read out from drive
#define _DRIVEAXISDIAG_WAITTIME_GETIFPOS_END       80  //waittime until I-FPOS is read out from drive
#define _DRIVEAXISDIAG_WAITTIME_SETANGLE           20  //waittime for set angle at drive
#define _DRIVEAXISDIAG_WAITTIME_COMMAND            10  //waittime for read or write command
#define _DRIVEAXISDIAG_WAITTIME_RAMPCURRENT        20  //waittime for increase current ramp
#define _DRIVEAXISDIAG_WAITTIME_AXENABLE          200  //waittime for axis enable

// Firmwareversions which support feedback determinination variant 1 (I-FBOFF) or variant 2 (G-Mode -6) or both
#define _DRIVEAXISDIAG_SDDXXX_SUPPORT_VARIANT_1_2     190   //version as decimal number 190 = v 1.90
#define _DRIVEAXISDIAG_SDDXXX_SUPPORT_VARIANT_2       189
#define _DRIVEAXISDIAG_MDDXXX_SUPPORT_VARIANT_1_2     186
#define _DRIVEAXISDIAG_DC06X_SUPPORT_VARIANT_1_2      350
#define _DRIVEAXISDIAG_WA012_SUPPORT_VARIANT_1_2      370

#define _DRIVEAXISDIAG_SR01X_SUPPORT_VARIANT_1_2      000   //not supported
#define _DRIVEAXISDIAG_SDD1X00_SUPPORT_VARIANT_1_2    000   //not supported
//}}LSL_DEFINES

//{{LSL_DECLARATION
#include "..\..\Class\MDD2000MngBase\MDD2000MngBase.h"

(*!
<Class
	Name               = "MDD2000_DriveAxisDiag"
	Revision           = "1.0"
	GUID               = "{D050AA78-E215-4E4D-9C62-32334D740E45}"
	RealtimeTask       = "false"
	CyclicTask         = "true"
	DefCyclictime      = "10 ms"
	BackgroundTask     = "false"
	Sigmatek           = "true"
	OSInterface        = "false"
	HighPriority       = "false"
	Automatic          = "false"
	UpdateMode         = "Prescan"
	IconPath           = ".\Class\MDD2000_Mng\MDD2000SafetyDrive.ico"
	SharedCommandTable = "true"
	Objectsize         = "(810,1200)"
	Comment            = "This class is used for the motor diagnosis.&#13;&#10;IMPORTANT! &#10;When determining the amount of motor poles or feedback offset, it is essential that the motor is &#10;not flange-mounted to the machine, as the motor could possibly pass through. ">
	<Channels>
		<Server Name="AxisErrorState" GUID="{BEA31515-9532-472B-82E7-EA28EF0B73E7}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the state of axis (I-STATUS P199). &#13;&#10;The value is only read asynchrone when a StartDiag command is used, &#13;&#10;or a QuitError is executed if there is an error on the axis.&#13;&#10;For further information look at the according drive help.&#13;&#10;First bit of lasal is Bit 1 but first bit of drive help is Bit 0."/>
		<Server Name="CurrentReduction" GUID="{088DCDE9-B26F-401A-A2D0-860C7760C8F4}" Visualized="true" Initialize="true" DefValue="100" WriteProtected="false" Retentive="false" Comment="Can be used to decrease the current used to perform the measurements. (1-100%, default: 100%)&#13;&#10;100% .. the smaller value of M-INULL and I-ICONT is used&#13;&#10; 50%  .. 50% of the smaller value of M-INULL and I-ICONT is used&#13;&#10;&#13;&#10;Needs to be set befor starting with StartDiag to take effect."/>
		<Server Name="DiagState" GUID="{D0E13345-E8B9-4816-8A16-E7F5EC4A2C38}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Displays the current status of the drive axis diagnostics class. The possible states are:&#13;&#10;_IdleDiag ... Motor diagnosis is in idle state&#13;&#10;_GetAxisPara .. All necessary motor parameters are read from the axis.&#13;&#10;_DiagGetAmountOfPoles .. Determining of the motor poles&#13;&#10;_DiagGetFeedbackOffset ... Determining the Feedback Offset&#13;&#10;_DiagMoveAxis .. Axis is moved to next test position.&#13;&#10;_SetAxisPara ... Motor parameters are set to values before diag was started.&#13;&#10;_SetMROFFPara .. Motor parameter M-ROFF is written to the axis.&#13;&#10;_SetMPOLPara .. Motor parameter M-POL is written to the axis.&#13;&#10;_QuitAxisError .. Axis error is acknowledged&#13;&#10;_FinishedDiag .. diagnosis is finished.&#13;&#10;_ErrorDiag ... An error has occurred during motor diagnosis.&#13;&#10;_DriveAxisNotOnline .. _DriveAxis object is not online&#13;&#10;_ErrorStartDiag .. Error when entering &quot;StartDiag&quot; on server (invalid command or command not possible in this state)&#13;&#10;_NotSupported .. G-Mode -6 for motor diagnosis is not supported (a newer drive firmware is needed)."/>
		<Server Name="DirectionMeasured" GUID="{F0B0C426-C421-4707-A37C-9A982E93B7B8}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the direction of movement.&#13;&#10;This value is only valid if MotorPolesOK = 1 or 2&#13;&#10;and MotorPhasesOK = 1.&#13;&#10; &#13;&#10;0 .. negative direction&#13;&#10;1 .. positive direction"/>
		<Server Name="FeedbackMaxDeviation" GUID="{CBFEB918-4D5E-4F84-8614-0E8622241722}" Visualized="true" Initialize="true" DefValue="5" WriteProtected="false" Retentive="false" Comment="Determines how large the mechanical deviation of the feedback offset in [°] may be. &#13;&#10;The default value is 5° mechanical angle.&#13;&#10;For example 5° mechanical angle for a 6 pole motor: (6 pol / 2) * 5° = 15° electrical angle.&#13;&#10;Possible values are from 0 ... 360°"/>
		<Server Name="FeedbackOffsetMeasured" GUID="{C7212A7D-6ACF-4458-A959-52A5A1A829A0}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the measured value of feedback offset from axis (I-FBOFF).&#13;&#10;This value can be set at the drive using the StartDiag server.&#13;&#10;This value is only valid if FeedbackOffsetOK = 1 or 2."/>
		<Server Name="FeedbackOffsetOK" GUID="{D7BD329D-4268-4CD8-9C71-90A74F61DE2C}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the state of feedback offset determining.&#13;&#10;In case of an axis error it is possible to read out the detailed error reason on the server AxisErrorState.&#13;&#10; 2 .. feedback offset measure is ok but measured feedback offset is not in the allowed range from FeedbackMaxDeviation to M-ROFF parameter at axis.&#13;&#10; 1 .. feedback offset measure is ok, measured feedback offset is in the allowed range from FeedbackMaxDeviation to M-ROFF parameter at axis.&#13;&#10; 0 .. measure not started or new measure started&#13;&#10;-1 .. one measured feedback offset is not within the permitted range (server FeedbackMaxDeviation).&#13;&#10;-2 .. a general error at communication with axis occurs (Async communication failed).&#13;&#10;-3 .. feedback offset measure failed because M-POL of axis is set to zero&#13;&#10;-4 .. Parameter I-FBOFF for feedback determinaton is not supported (a newer drive firmware is needed).&#13;&#10;-5 .. feedback or motorphase error&#13;&#10;-6 .. There is an error on the axis that cannot be acknowledged. The error is displayed on the AxisErrorState server.&#13;&#10;-7 .. motor poles on axis is not the same as measured motor poles&#13;&#10;-8 .. motor direction is not ok (change two motor phases)"/>
		<Server Name="FeedbackOffsetOnAxis" GUID="{0F8E4F20-ACF3-4F2C-9867-A77A0F8B3C22}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the feedback offset read from axis (M-ROFF P014). &#13;&#10;The value is only read asynchrone when a StartDiag command is used."/>
		<Server Name="MotorPhasesOK" GUID="{16C751A5-E9D6-4D08-BC95-FCA05378CB70}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Show the state of motor phase check.&#13;&#10;In case of an axis error it is possible to read out the detailed error reason on the server AxisErrorState.&#13;&#10; 1 .. motor phases are ok&#13;&#10; 0 .. measure not started or new measure started&#13;&#10;-1 .. motor phase is not ok (missing phase or feedback error)&#13;&#10;-2 .. motor direction is not ok (change two motor phases)"/>
		<Server Name="MotorPolesMeasured" GUID="{EBF4D1F9-4393-483A-A85F-CBCC358112EC}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the measured value of motor poles (M-POL).&#13;&#10;This value can be set at the drive using the StartDiag server.&#13;&#10;This value is only valid if MotorPolesOK = 1 or 2."/>
		<Server Name="MotorPolesOK" GUID="{B74D841D-54ED-4C0A-B5C4-73D299C29065}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the state of motor pole check. &#13;&#10;In case of an axis error it is possible to read out the detailed error reason on the server AxisErrorState.&#13;&#10; 2 .. motor poles measure is ok but not the same as set at axis for M-POL parameter. &#13;&#10;       The measured value can be written to axis with StartDiag server.&#13;&#10; 1 .. motor poles measure is ok, measured value is the same as set at axis for M-POL parameter&#13;&#10; 0 .. measure not started or new measure started&#13;&#10;-1 .. motor phase is not ok (missing phase or feedback error)&#13;&#10;-2 .. determined motor poles are not in a valid range (between 2-256 and divisible by 2) &#13;&#10;-3 .. a general error at communication with drive occurs (Async communication failed).&#13;&#10;-4 .. G-Mode -6 for motor pole determinaton is not supported (a newer drive firmware is needed).&#13;&#10;-5 .. There is an error on the axis that cannot be acknowledged. The error is displayed on the AxisErrorState server."/>
		<Server Name="MotorPolesOnAxis" GUID="{2C14B935-6F3C-4BC1-B361-DEF18F2774AA}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the amount of motor poles read from axis (M-POL P007).&#13;&#10;The value is only read when a StartDiag command is used.&#13;&#10;The value is only read asynchrone when a StartDiag command is used."/>
		<Server Name="QuitError" GUID="{23236817-0685-4928-9EED-25102AF0736A}" Visualized="true" Initialize="false" WriteProtected="false" Retentive="false" Comment="If the DiagState, AxisErrorState, MotorPolesOK or FeedbackOffsetOK  is in an error state , this server reset it. &#13;&#10;If the axis is in an error state (AxisErrorState), this is also acknowledged.&#13;&#10;"/>
		<Server Name="StartDiag" GUID="{BEC2A7A8-F897-4EBD-A75B-58EB1E76B361}" Visualized="true" Initialize="false" WriteProtected="false" Retentive="false" Comment="Start the motor diagnosis or write the measured values for M-POL or M-ROFF to axis&#13;&#10;IMPORTANT! &#10;When determining the amount of motor poles or feedback offset, it is essential &#13;&#10;that the motor is &#10;not flange-mounted to the machine, as the motor could possibly pass through.&#13;&#10;&#13;&#10;During determinig of motor poles or feedback offset and if one of these tests fails, the _DriveAxis object is locked for &#13;&#10;other accesses! The _DriveAxis object will only be unlocked again when the tests have been carried out without errors.&#13;&#10;Start of motor diagnosis or write of measured value is only possible if StartDiag has the _IdleStartDiag state.&#13;&#10;&#13;&#10;Possible commandos are:&#13;&#10;&#13;&#10;_GetAmountOfMotorPoles: &#13;&#10;Motor phases and direction is also checked&#13;&#10;&#13;&#10;_SetMeasuredMotorPolesAtAxis, (Parameter M-POL P007):&#13;&#10;Select this to write the value of MotorPolesMeasured at axis.&#13;&#10;If MotorPolesOk &lt;&gt; 1 or 2 it is not possible to set the value at axis.&#13;&#10;Depending on the client ParaSaveType of the respective _DriveAxis object the parameter is stored on the axis.&#13;&#10;&#13;&#10;_GetFeedbackOffset:&#13;&#10;Start of determining feedback offset is only possible if motor pole test is ok &#13;&#10;or was not started (MotorPolesOK = 0 or 1).&#13;&#10;&#13;&#10;_SetMeasuredFeedbackOffsetAtAxis (Parameter M-ROFF P014)&#13;&#10;Select this to write the value of FeedbackOffsetMeasured at axis.&#13;&#10;If FeedbackOffsetOk &lt;&gt; 1 or 2 and MotorPolesOk &lt;&gt; 0 oder 1 it is not possible to set the value at axis.&#13;&#10;Depending on the client ParaSaveType of the respective _DriveAxis object the parameter is stored on the axis."/>
		<Server Name="StopDiag" GUID="{A67F6B96-D69A-4895-93F2-5F13E3D59450}" Visualized="true" Initialize="false" WriteProtected="false" Retentive="false" Comment="With write to this server the actual diagnosis operation can be stopped.&#13;&#10;"/>
		<Server Name="UsedCurrent" GUID="{AA1B715F-38FA-4F8E-8F35-660A6A2503E3}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Displays the current used to perform the measurements. (The value is determined with StartDiag).&#13;&#10;Depends on M-INULL, I-ICONT and CurrentReduction."/>
	</Channels>
	<Separators>
		<Servers>
			<SepChn Position="13"/>
			<SepChn Position="4"/>
			<SepChn Position="18"/>
			<SepChn Position="7"/>
		</Servers>
	</Separators>
	<Dependencies>
		<Files>
			<File Path=".\Class\MDD2000MngBase\MDD2000MngBase.h" Include="true"/>
		</Files>
	</Dependencies>
	<RevDoku>
		<Owner Company="Sigmatek" Author="LanSte"/>
		<Dokumentation Revision="1.0" Date="28.08.2023" Author="LanSte" Company="Sigmatek" Description="First library version."/>
	</RevDoku>
	<Network Name="MDD2000_DriveAxisDiag">
		<!-- List of Components in this network -->
		<Components>
			<Object
				Name       = "_base"
				GUID       = "{AE99DB62-101A-42F2-B953-71ED905A66AE}"
				Class      = "_DriveSafetyIFParaBase"
				Position   = "(218,120)"
				Visualized = "true"
				Remotely   = "true">
				<Channels>
					<Server Name="ErrorQuit"/>
					<Server Name="State"/>
					<Client Name="DriveIF"/>
					<Client Name="ErrorCode"/>
					<Client Name="UpdateCyTime" Value="0"/>
				</Channels>
			</Object>
		</Components>
		<Comments>
		</Comments>
		<!-- List of Connections in this network -->
		<Connections>
			<Connection Source="this.State" Destination="_base.State" Vertices="(740,210),(568,210),"/>
			<Connection Source="this.ErrorQuit" Destination="_base.ErrorQuit" Vertices="(740,270),(568,270),"/>
			<Connection Source="_base.DriveIF" Destination="this.DriveIF" Vertices="(218,210),(38,210),"/>
			<Connection Source="_base.ErrorCode" Destination="this.ErrorCode" Vertices="(218,270),(38,270),"/>
		</Connections>
		<!-- Headerfiles -->
		<Options>
		</Options>
	</Network>
</Class>
*)
#pragma using _DriveSafetyIFParaBase

MDD2000_DriveAxisDiag : CLASS
: _DriveSafetyIFParaBase
	TYPE
	  bdDriveParameter : BDINT
	  [
	    1 P004_M_INULL,
	    2 P007_M_POL,
	    3 P012_M_TYPE,
	    4 P013_M_RPOL,
	    5 P014_M_ROFF,
	    6 P015_M_RPULSE,
	    7 P032_G_MODE,
	    8 P173_A_ACME,
	    9 P198_I_NFILT,
	    10 Bit10,
	    11 Bit11,
	    12 Bit12,
	    13 Bit13,
	    14 Bit14,
	    15 Bit15,
	    16 Bit16,
	    17 Bit17,
	    18 Bit18,
	    19 Bit19,
	    20 Bit20,
	    21 Bit21,
	    22 Bit22,
	    23 Bit23,
	    24 Bit24,
	    25 P220_K_EN_AxDisable,
	    26 P226_K_CI_Reset,
	    27 P232_K_PHI_Reset,
	    28 Bit28,
	    29 Bit29,
	    30 Bit30,
	    31 Bit31,
	    32 Bit32,
	  ];
	  biDiagErrorFlags : BINT
	  [
	    1 MotorPolesPhaseError,
	    2 MotorPolesOnAxisZero,
	    3 MotorDirectionFalse,
	    4 MotorPolesToAxisDiff,
	    5 MotorPolesUnallowedValue,
	    6 DiagNotSupportedFW,
	    7 AxErrorNotQuitable,
	    8 Bit8,
	    9 FeedbackMeasureLimits1,
	    10 FeedbackMeasureLimits2,
	    11 FeedbackOffsetOnAxis,
	    12 Bit12,
	    13 Bit13,
	    14 Bit14,
	    15 Bit15,
	    16 Bit16,
	  ];
	  pHwcLogging : ^HwcLogging;
	  teSSWDiag :  //! <Type Public="true" Name="teSSWDiag"/>
	  (
	    _IdleDiag,
	    _GetDriveType,
	    _GetFWVersion,
	    _ReadAxisPara,
	    _ReadBackAxisPara,
	    _DiagGetAmountOfPoles,
	    _DiagGetFeedbackOffset,
	    _DiagMoveAxis,
	    _SetAxisPara,
	    _SetMROFFPara,
	    _SetMPOLPara,
	    _QuitAxisError,
	    _FinishedDiag,
	    _ErrorDiag,
	    _AxisNotOnline,
	    _ErrorStartDiag,
	    _NotSupported
	  )$UDINT;
	  teSSWDiagControl :  //! <Type Public="true" Name="teSSWDiagControl"/>
	  (
	    _IdleStartDiag,
	    _GetAmountOfMotorPoles,
	    _SetMeasuredMotorPolesAtAxis,
	    _GetFeedbackOffset,
	    _SetMeasuredFeedbackOffsetAtAxis
	  )$UDINT;
	  teSSWMotorDiag :
	  (
	    _MotorDiagIdle,
	    _Set_K_CLRF,
	    _Get_I_STATUS,
	    _Reset_M_POL,
	    _Reset_M_ROFF,
	    _Set_A_ACME,
	    _Set_G_Mode,
	    _SetStartCurrent,
	    _SetStartAngle,
	    _AxisEnable,
	    _Wait4AxisEnable,
	    _SetRampCurrent,
	    _Wait4SetRampCurrent,
	    _Get_I_FPOS_Start,
	    _Get_I_FBOFF,
	    _SetElectricAngle,
	    _Wait4SetAngle,
	    _Get_I_FPOS_End,
	    _AxisDisable,
	    _CheckValues,
	    _CheckValues2,
	    _MotorDiagFinished,
	    _MotorDiagError
	  )$UDINT;
	  teSSWParaAxis :
	  (
	    _ParaIdle,
	    _P004_M_INULL,
	    _P186_I_ICONT,
	    _P007_M_POL,
	    _P012_M_TYPE,
	    _P013_M_RPOL,
	    _P014_M_ROFF,
	    _P015_M_RPULSE,
	    _P032_G_MODE,
	    _P173_A_ACME,
	    _P180_I_FBOFF,
	    _P199_I_STATUS,
	    _P220_K_EN_Reset,
	    _P226_K_CI_Reset,
	    _P232_K_PHI_Reset,
	    _ParaFinished,
	    _ParaError
	  )$UDINT;
	  teSSWSDOHandling :
	  (
	    _IdlePara,
	    _RdWrPara,
	    _Wait4Data
	  )$UDINT;
#pragma pack(push, 1)
	  tsAxisPara : STRUCT
	    dP004_M_INULL : DINT;
	    dP007_M_POL : DINT;
	    dP012_M_TYPE : DINT;
	    dP013_M_RPOL : DINT;
	    dP014_M_ROFF : DINT;
	    dP015_M_RPULSE : DINT;
	    dP032_G_MODE : DINT;
	    dP173_A_ACME : DINT;
	    dP180_I_FBOFF : DINT;
	    dP186_I_ICONT : DINT;
	    dP187_I_FW : DINT;
	    dP198_I_NFILT : DINT;
	    dP199_I_STATUS : DINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  tsFeedbackValues : STRUCT
	    dM_ROFF : DINT;
	    dI_FBOFF : DINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  tsPositionInfo : STRUCT
	    dStartPos : DINT;
	    dEndPos : DINT;
	    udDiffPos : UDINT;
	    dTmpDiffPos : DINT;
	    dLastPos : DINT;
	    usPosChangeErrorCnt : USINT;
	    usDirectionErrorCnt : USINT;
	  END_STRUCT;
#pragma pack(pop)
	END_TYPE
  //Servers:
	AxisErrorState 	: SvrCh_BDINT;
	DiagState 	: SvrCh_teSSWDiag_PTofCls_MDD2000_DriveAxisDiag;
	StartDiag 	: SvrCh_teSSWDiagControl_PTofCls_MDD2000_DriveAxisDiag;
	StopDiag 	: SvrCh_DINT;
	CurrentReduction 	: SvrCh_DINT;
	UsedCurrent 	: SvrCh_DINT;
	MotorPolesOK 	: SvrCh_DINT;
	MotorPhasesOK 	: SvrCh_DINT;
	MotorPolesMeasured 	: SvrCh_DINT;
	DirectionMeasured 	: SvrCh_DINT;
	MotorPolesOnAxis 	: SvrCh_DINT;
	FeedbackOffsetOK 	: SvrCh_DINT;
	FeedbackOffsetMeasured 	: SvrCh_DINT;
	FeedbackOffsetOnAxis 	: SvrCh_DINT;
	FeedbackMaxDeviation 	: SvrCh_DINT;
	QuitError 	: SvrCh_DINT;
  //Clients:
  //Variables:
		sDiagSSW 	: teSSWDiag;
		sDiagErrorStep 	: teSSWDiag;
		sMotorDiagSSW 	: teSSWMotorDiag;
		sGetMPolesErrorStep 	: teSSWMotorDiag;
		sGetFeedbackOffsetErrorStep 	: teSSWMotorDiag;
		sMoveAxisAngleErrorStep 	: teSSWMotorDiag;
		sParaAxisSSW 	: teSSWParaAxis;
		sParaAxisErrorStep 	: teSSWParaAxis;
		sParaSDOSSW 	: teSSWSDOHandling;
		sAxisParameter 	: tsAxisPara;
		sPositionInfo 	: tsPositionInfo;
		aFeedbackOffset : ARRAY [0.._DRIVEAXISDIAG_FEEDBACK_MEASURE_POINTS-1] OF tsFeedbackValues;

		udStartWaitTime 	: UDINT;
		dActAngleDec 	: DINT;
		bLastElecTurn 	: BOOL;
		dMoveAxisAngleCnt 	: DINT;
		dTmpDirection 	: DINT;
		dPoleMeasurePoints 	: DINT;
		bDiagModeStarted 	: BOOL;
		bFinishedPoleMeasure 	: BOOL;
		bStopDiag 	: BOOL;
		dRampCurrent 	: DINT;
		dRampCurrentOneStep 	: DINT;
		dMaxPositionDeviation 	: DINT;
		biErrorFlags 	: biDiagErrorFlags;
		bdSetOldValues 	: bdDriveParameter;
		bLoggingNotSupported 	: BOOL;
		dFeedbackMaxElectricDeviation 	: DINT;
		bGetDriveType 	: BOOL;
		bsSupportedDiagMode : BSINT
		[
		  1 FeedbackOffset,
		  2 MotorPoles,
		];

		bsFeedbackValidFlag : BSINT
		[
		  1 MeasuredFeedback1,
		  2 MeasuredFeedback2,
		];

  //Functions:
	
	FUNCTION VIRTUAL GLOBAL Init;
	
	FUNCTION VIRTUAL GLOBAL CyWork
		VAR_INPUT
			EAX 	: UDINT;
		END_VAR
		VAR_OUTPUT
			state (EAX) 	: UDINT;
		END_VAR;
	
	FUNCTION WriteParameter
		VAR_INPUT
			ParaID 	: DINT;
			ParaSubID 	: DINT;
			ParamValue 	: DINT;
			EntryType 	: USINT := 0;			//! <Variable Comment="Sets the mode for an update after executing the command&#13;&#10;&#13;&#10;0..Default&#13;&#10;1..all connected parameterclasses for this axis get refreshed after the command&#13;&#10;2..Indicates if the first part of the refresh from the paratmerclasses is finished&#13;&#10;3..only used by Lasal2" Name="WriteParameter.EntryType"/>
		END_VAR
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION ReadParameter
		VAR_INPUT
			ParaID 	: DINT;
			ParaSubID 	: DINT;
			pParamValue 	: ^DINT;
			EntryType 	: USINT := 0;
		END_VAR
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION GetAxisPara
		VAR_OUTPUT
			dRetcode 	: iprStates;
		END_VAR;
	
	FUNCTION SetAxisPara
		VAR_OUTPUT
			dRetcode 	: iprStates;
		END_VAR;
				//! <Function Comment="Methode for measure the amount of motor poles from axis.&#13;&#10;&#13;&#10;1.)G-MODE (P32) -6: Service mode for setting the electrical angle&#13;&#10;    The following two parameters are relevant in this mode:&#13;&#10;    - K-CI (P226) ... Presetting the nominal current&#13;&#10;    - K-PHI (P232) ... new parameter for setting the electrical target angle. The value range is [0 ... 65535] which corresponds to [0 ... 360°[&#13;&#10;   &#13;&#10;    With the help of these modes it is possible, for example, to determine the number of motor poles of the connected motor. You could proceed as follows:&#13;&#10;    1.) Enable axis&#13;&#10;    2.) Set target angle to 0 and target current to M-INULL/2&#13;&#10;    3.) Wait until the rotor has aligned itself.&#13;&#10;    4.) Remember value of I-FPOS&#13;&#10;    5.) Move through the electrical nominal angle in e.g. 30° steps up to 360°.&#13;&#10;    6.) Leave angle at 360° and read new value from I-FPOS&#13;&#10;    7.) Determining the difference between the two I-FPOS values&#13;&#10;    8.) The number of motor pole pairs can be determined from the difference:&#13;&#10;    If the difference is e.g. approx. 2^32/5, it is a motor with 5 pole pairs.&#13;&#10;    Alternatively, a full mechanical revolution could also be carried out with several electrical runs from 0 to 360°. &#13;&#10;    to then obtain the number of motor poles from the number of required electrical runs." Name="GetMotorPoles"/>
	FUNCTION GetMotorPoles
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
				//! <Function Comment="Methode for measure of feedback offset from axis.&#13;&#10;&#13;&#10;1. Offset calculation according to the Application Notes&#13;&#10;&#13;&#10; Calculation M-ROFF in the class&#13;&#10; IMPORTANT!!!&#13;&#10; When determining the feedback offset it is essential to ensure that the motor is not flanged to the machine, as the motor could possibly pass through. &#13;&#10; The determination of the feedback offset should be repeated at least three times at different positions and the determined values compared. &#13;&#10; It can happen that the motor stops exactly between two phases and that the feedback offset determined is wrong by 180°.&#13;&#10; &#13;&#10; * Check the feedback direction of rotation. If the motor shaft is driven in positive direction (clockwise), the actual speed (parameter I-NFILT) must be positive.&#13;&#10; * Setting the correct number of resolver poles (parameter M-RPOL) or the correct feedback pulses (parameter M-RPULSE), depending on the feedback system used. &#13;&#10; * Set the number of motor poles (parameter M-POL) to 0. &#13;&#10; * If a feedback offset (parameter M-ROFF) has already been set, set it back to the default value 0. !!! not necessary with G-MODE -6&#13;&#10; * Set the Current Controller Service Mode (parameter G-MODE = -1).&#13;&#10;&#13;&#10; !!! Rated motor current is increased up to 100% (agreed with Gradl)&#13;&#10; Set the current set (parameter K-CI) to 30 % of the rated motor current. * Enable motor (parameter K-EN = 1). Attention: Motor turns abruptly to a fixed position! * Read in the actual value I-FPOS. * Disable motor (parameter K-EN = 0) M-ROFF = (90°-(I-FPOS*(360/2^32))*(M-POL/2)+-(x*360)&#13;&#10; x so that the value lies between 0° and 360°. This is then the correct value for the parameter M-ROFF. If the M-POL is negative, the formula is as follows: M-ROFF = (-90°-(I-FPOS*(360/2^32))*(M-POL/2)+-(x*360)&#13;&#10; * Set the correct value for the number of motor poles (parameter M-POL). * Set the current set (parameter K-CI) to 0. Set to the desired operating mode. Enable motor - Motor must now rotate. If not, the motor phase sequence is not correct. In this case, two motor phases must be swapped. The setting procedure starts again by setting the number of motor poles to the value 0.&#13;&#10;&#13;&#10; &#13;&#10;2.  Offset calculation via axis parameter I-FBOFF&#13;&#10;&#13;&#10; Calculation M-ROFF determined from the axis via I-FBOFF&#13;&#10; (Determination at two pole pairs is sufficient)&#13;&#10; 1. remember current G-MODE&#13;&#10; 2. set G-MODE = -6&#13;&#10; 3. set K-PHI (P232) = 0&#13;&#10; 4. set K-CI = 0&#13;&#10; Enable 5th axis&#13;&#10; 6. K-CI -&gt; Ramp from 0 to min(M-INULL, I-ICONT) in adjustable time (default value 500ms)&#13;&#10; 7. waiting time 1s if mechanical oscillation occurs&#13;&#10; 8. read and memorize I-FBOFF (P180) (Offset_1)&#13;&#10; 9. read and memorize I-FPOS (Pos_1)&#13;&#10; 10. increase K-PHI in adjustable time (Deaul value 1s) to 65535&#13;&#10; 11. set K-PHI = 0&#13;&#10; 12. waiting time 100ms&#13;&#10; 13. read out I-FBOFF (Offset_2)&#13;&#10; 14. read I-FPOS (Pos_2)&#13;&#10; Disable 15th axis&#13;&#10; 16. check whether the offset differs mechanically by more than 5°: abs(Offset_1 - Offset_2) / (M-POL / 2) &lt;= 5° Attention with offsets of 0° / 359°!&#13;&#10; 17. check whether the feedback position has continued to rotate positively by 360°/(M-POL/2): abs(Pos_1 - Pos_2) / (M-POL / 2) &lt;= 2^32 * 5 / 360. 2^32 * 5 / 360 corresponds to 5° mechanically, because I-FPOS is output in 2^32 per revolution.&#13;&#10; 18. if checks OK -&gt; M-ROFF = abs(Offset_1 + Offset_2) / 2&#13;&#10; Attention again with offsets around 0° / 359° !!)&#13;&#10; 19. reset G-MODE to the memorized mode&#13;&#10; " Name="GetFeedbackOffset"/>
	FUNCTION GetFeedbackOffset
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
				//! <Function Comment="Methode to move axis." Name="MoveAxisAngle"/>
	FUNCTION MoveAxisAngle
		VAR_INPUT
			dAngle 	: DINT;
		END_VAR
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION QuitAxError
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION VIRTUAL CheckFWVersion
		VAR_OUTPUT
			dRetCode 	: DINT;
		END_VAR;
	
	FUNCTION GLOBAL LogError
		VAR_INPUT
			pMsg 	: ^CHAR;
		END_VAR;
	
	FUNCTION SetDiagMode
		VAR_INPUT
			bStartDiagMode 	: BOOL;
			pThis 	: ^void;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL StartDiag::Write
		VAR_INPUT
			input (EAX) 	: MDD2000_DriveAxisDiag::teSSWDiagControl;
		END_VAR
		VAR_OUTPUT
			result (EAX) 	: MDD2000_DriveAxisDiag::teSSWDiagControl;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL StopDiag::Write
		VAR_INPUT
			input (EAX) 	: DINT;
		END_VAR
		VAR_OUTPUT
			result (EAX) 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL CurrentReduction::Write
		VAR_INPUT
			input (EAX) 	: DINT;
		END_VAR
		VAR_OUTPUT
			result (EAX) 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL FeedbackMaxDeviation::Write
		VAR_INPUT
			input (EAX) 	: DINT;
		END_VAR
		VAR_OUTPUT
			result (EAX) 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL QuitError::Write
		VAR_INPUT
			input (EAX) 	: DINT;
		END_VAR
		VAR_OUTPUT
			result (EAX) 	: DINT;
		END_VAR;
  //Tables:
	FUNCTION @STD
		VAR_OUTPUT
			ret_code	: CONFSTATES;
		END_VAR;
	FUNCTION GLOBAL TAB @CT_;
END_CLASS;

//}}LSL_DECLARATION


FUNCTION GLOBAL TAB MDD2000_DriveAxisDiag::@CT_
0$UINT,
2#0100000000000000$UINT, //TY_MDD2000_DRIVEAXISDIAG
1$UINT, 0$UINT, (SIZEOF(::MDD2000_DriveAxisDiag))$UINT, 
16$UINT, 0$UINT, 0$UINT, 
TO_UDINT(701686548), "MDD2000_DriveAxisDiag", //Class
TO_UDINT(1914213293), "_DriveSafetyIFParaBase", 1$UINT, 0$UINT, //Baseclass
//Servers:
(::MDD2000_DriveAxisDiag.AxisErrorState.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1086760845), "AxisErrorState", 
(::MDD2000_DriveAxisDiag.DiagState.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(828455980), "DiagState", 
(::MDD2000_DriveAxisDiag.StartDiag.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(2029320521), "StartDiag", 
(::MDD2000_DriveAxisDiag.StopDiag.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3320529608), "StopDiag", 
(::MDD2000_DriveAxisDiag.CurrentReduction.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1988973820), "CurrentReduction", 
(::MDD2000_DriveAxisDiag.UsedCurrent.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(2001036017), "UsedCurrent", 
(::MDD2000_DriveAxisDiag.MotorPolesOK.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(578393205), "MotorPolesOK", 
(::MDD2000_DriveAxisDiag.MotorPhasesOK.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(2180666259), "MotorPhasesOK", 
(::MDD2000_DriveAxisDiag.MotorPolesMeasured.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(632089324), "MotorPolesMeasured", 
(::MDD2000_DriveAxisDiag.DirectionMeasured.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1815569134), "DirectionMeasured", 
(::MDD2000_DriveAxisDiag.MotorPolesOnAxis.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(4129040359), "MotorPolesOnAxis", 
(::MDD2000_DriveAxisDiag.FeedbackOffsetOK.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(311858428), "FeedbackOffsetOK", 
(::MDD2000_DriveAxisDiag.FeedbackOffsetMeasured.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3060013826), "FeedbackOffsetMeasured", 
(::MDD2000_DriveAxisDiag.FeedbackOffsetOnAxis.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(2574385625), "FeedbackOffsetOnAxis", 
(::MDD2000_DriveAxisDiag.FeedbackMaxDeviation.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(559797366), "FeedbackMaxDeviation", 
(::MDD2000_DriveAxisDiag.QuitError.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3032567726), "QuitError", 
//Clients:
END_FUNCTION


#define USER_CNT_MDD2000_DriveAxisDiag 20

TYPE
	_LSL_STD_VMETH	: STRUCT
			CmdTable	: CMDMETH;
			UserFcts	: ARRAY[0..USER_CNT_MDD2000_DriveAxisDiag] OF ^Void;
	END_STRUCT;
END_TYPE

FUNCTION MDD2000_DriveAxisDiag::@STD
	VAR_OUTPUT
		ret_code	: CONFSTATES;
	END_VAR
	VAR
		vmt	: _LSL_STD_VMETH;
		nCmdSize	: UINT;
	END_VAR

	ret_code	:= _DriveSafetyIFParaBase::@STD();
	IF ret_code <> C_OK THEN
		RETURN;
	END_IF;
	nCmdSize		:= _DriveSafetyIFParaBase::State.pMeth^.nCmds$UINT*SIZEOF(pVoid) + CMDMETH.Init;

	_memcpy((#vmt.CmdTable)$^USINT, _DriveSafetyIFParaBase::State.pMeth, nCmdSize);
	vmt.CmdTable.nCmds		:= nSTDCMD + USER_CNT_MDD2000_DriveAxisDiag;
	vmt.CmdTable.Init		:= #Init();
	vmt.CmdTable.CyWork		:= #CyWork();
#pragma warning (disable : 74)
	vmt.UserFcts[19]		:= #CheckFWVersion();

#pragma warning (default : 74)
	_DriveSafetyIFParaBase::State.pMeth		:= StoreCmd (pCmd := #vmt.CmdTable, SHARED);

	IF _DriveSafetyIFParaBase::State.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	StartDiag.pMeth			:= StoreMethod( #M_RD_DIRECT(), #StartDiag::Write() );
	IF StartDiag.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	StopDiag.pMeth			:= StoreMethod( #M_RD_DIRECT(), #StopDiag::Write() );
	IF StopDiag.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	CurrentReduction.pMeth			:= StoreMethod( #M_RD_DIRECT(), #CurrentReduction::Write() );
	IF CurrentReduction.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	FeedbackMaxDeviation.pMeth			:= StoreMethod( #M_RD_DIRECT(), #FeedbackMaxDeviation::Write() );
	IF FeedbackMaxDeviation.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	QuitError.pMeth			:= StoreMethod( #M_RD_DIRECT(), #QuitError::Write() );
	IF QuitError.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;

END_FUNCTION

#pragma usingLtd HwcLogging

//{{LSL_IMPLEMENTATION
FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::Init
  
  //call init from base class
  _DriveSafetyIFParaBase::Init();
  
  if _FirstScan then
    //check init value for max feedback deviation
    FeedbackMaxDeviation.Write(input:=FeedbackMaxDeviation);
  end_if;
  
END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::CyWork
	VAR_INPUT
		EAX 	: UDINT;
	END_VAR
	VAR_OUTPUT
		state (EAX) 	: UDINT;
	END_VAR
  VAR
  	dRetCode : iprStates;
    dTmpRetCode : DINT;
  END_VAR  
  
  if IsAxisOnline() then
    case sDiagSSW of
      //**************************************************************
      teSSWDiag::_IdleDiag:
        if bGetDriveType = FALSE then          
          sDiagSSW := _GetDriveType;
        end_if;

      //**************************************************************
      teSSWDiag::_GetDriveType: 
      
        sDiagSSW := _GetFWVersion;
      
      //**************************************************************
      teSSWDiag::_GetFWVersion:       
        dRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_FW, pParamValue:=#sAxisParameter.dP187_I_FW);
        if dRetCode = READY then
          sDiagSSW      := _FinishedDiag;
          bGetDriveType := TRUE;
          
          //check version
          dTmpRetCode := CheckFWVersion();
          
          bsSupportedDiagMode := dTmpRetCode$BSINT;
          if bsSupportedDiagMode.MotorPoles = FALSE then 
            //angle mode -6 is not supported - new drive firmware is needed
            LogError("@05B7 (MDD2000_DriveAxisDiag::CyWork) G-Mode -6 is not supported (a newer drive firmware is needed).");
            MotorPolesOK  := -4;
            sDiagSSW      := _NotSupported; 
          end_if;
          if bsSupportedDiagMode.FeedbackOffset = FALSE then
            //Parameter I-FBOFF is not supported by drive FW 
            FeedbackOffsetOK := -4;
            LogError("@05FC (MDD2000_DriveAxisDiag::CyWork) Parameter I-FBOFF for feedback determinaton is not supported (a newer drive firmware is needed).");
          end_if;
          
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;
        
      
      //**************************************************************
      teSSWDiag::_ReadAxisPara, 
      teSSWDiag::_ReadBackAxisPara: 
        dRetCode := GetAxisPara();
        if dRetCode = READY then
          if sDiagSSW = teSSWDiag::_ReadAxisPara then
            case StartDiag of
              //********************************************************************************
              MDD2000_DriveAxisDiag::teSSWDiagControl::_GetAmountOfMotorPoles:
                sDiagSSW := _DiagGetAmountOfPoles; 
                if bsSupportedDiagMode.MotorPoles = FALSE then 
                  //angle mode -6 is not supported - new drive firmware is needed                    
                  MotorPolesOK  := -4;
                  sDiagSSW      := _NotSupported; 
                  StartDiag     := _IdleStartDiag;
                end_if;
                  
              //******************************************************************************** 
              MDD2000_DriveAxisDiag::teSSWDiagControl::_GetFeedbackOffset:
                          
                if bsSupportedDiagMode.FeedbackOffset = TRUE then                
                  if MotorPolesOK = 0 | MotorPolesOK = 1 then
                  
                    //get max electric feedback deviation from feedback max deviation (mechanical) and amount of Motorpoles
                    if MotorPolesOK = 1 & MotorPhasesOK = 1 then
                      dFeedbackMaxElectricDeviation := (MotorPolesMeasured/2)*FeedbackMaxDeviation;
                      dMaxPositionDeviation         := _DRIVEAXISDIAG_STEP_DEGREE_MECH/(MotorPolesMeasured*2);
                    elsif MotorPolesOnAxis then
                      dFeedbackMaxElectricDeviation := (MotorPolesOnAxis/2)*FeedbackMaxDeviation;
                      dMaxPositionDeviation         := _DRIVEAXISDIAG_STEP_DEGREE_MECH/(MotorPolesOnAxis*2);
                    else
                      //Amount of motor poles is not valid
                      FeedbackOffsetOK := -3;
                      //value for motor poles at axis is invalid
                      LogError("@05F9 (MDD2000_DriveAxisDiag::CyWork) Motorpoles of axis is set to zero which is no valid value!");
                      sDiagErrorStep := sDiagSSW;
                      sDiagSSW       := _ErrorDiag;                    
                      return;
                    end_if;
                    
                    if dFeedbackMaxElectricDeviation > _DRIVEAXISDIAG_MAX_ANGLE_DEGREE then
                      dFeedbackMaxElectricDeviation := _DRIVEAXISDIAG_MAX_ANGLE_DEGREE;
                    end_if;
            
                    dActAngleDec            := 0;
                    dMoveAxisAngleCnt       := 0;                  
                    //
                    dPoleMeasurePoints      := _DRIVEAXISDIAG_FEEDBACK_MEASURE_POINTS;
                    sDiagSSW                := _DiagGetFeedbackOffset;
                  else
                    if MotorPolesOnAxis then
                      //-9 .. motor poles on axis is not the same as measured motor poles
                      FeedbackOffsetOK := -7;
                      HWC_LogPrintf(pThis:=this, format:= "@05F7 (MDD2000_DriveAxisDiag::CyWork) Measured motor poles ( %u ) is not the same as M-POL on axis ( %u ).", #MotorPolesMeasured, #MotorPolesOnAxis);
                    else
                      //Amount of motor poles is not valid
                      FeedbackOffsetOK := -3;  
                      //value for motor poles at axis is invalid
                      LogError("@05F9 (MDD2000_DriveAxisDiag::CyWork) Motorpoles of axis is set to zero which is no valid value!");                  
                    end_if;
                    sDiagErrorStep := sDiagSSW;
                    sDiagSSW       := _ErrorDiag; 
                  end_if;  
                else
                  //mode is not supported
                  FeedbackOffsetOK := -4;
                  sDiagErrorStep := sDiagSSW;
                  sDiagSSW       := _ErrorDiag;
                end_if;                
                
              //********************************************************************************
              MDD2000_DriveAxisDiag::teSSWDiagControl::_SetMeasuredMotorPolesAtAxis:
                sDiagSSW := MDD2000_DriveAxisDiag::teSSWDiag::_SetMPOLPara;  
              //********************************************************************************
              MDD2000_DriveAxisDiag::teSSWDiagControl::_SetMeasuredFeedbackOffsetAtAxis:
                sDiagSSW := MDD2000_DriveAxisDiag::teSSWDiag::_SetMROFFPara;
            else
              sDiagSSW := _FinishedDiag;
            end_case;
          else
            //after readback of parameter we are finished 
            sDiagSSW := _FinishedDiag;  
          end_if;
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;
        
        
      //**************************************************************
      teSSWDiag::_DiagGetAmountOfPoles:
      
        dRetCode := GetMotorPoles();
        if dRetCode = READY then
          MotorPolesOK  := 1;
          MotorPhasesOK := 1;
          
          if biErrorFlags then
            if biErrorFlags.MotorPolesPhaseError then
              MotorPolesOK  := -1;
              MotorPhasesOK := -1; 
              LogError("@05F6 (MDD2000_DriveAxisDiag::CyWork)Motor phase or feedback is not ok (missing phase or feedback failure)!");
            end_if;
            
            if biErrorFlags.MotorPolesOnAxisZero then
              //value for motor poles at axis is invalid
              LogError("@05F9 (MDD2000_DriveAxisDiag::CyWork) Motorpoles of axis is set to zero which is no valid value!");
              MotorPolesOK := 2;
            end_if;
            
            if biErrorFlags.MotorPolesToAxisDiff then
              //measure of motor poles is ok but the value is not the same as on axis
              HWC_LogPrintf(pThis:=this, format:= "@05F7 (MDD2000_DriveAxisDiag::CyWork) Measured motor poles ( %u ) is not the same as M-POL on axis ( %u ).", #MotorPolesMeasured, #MotorPolesOnAxis);
              MotorPolesOK := 2;
            end_if;   
            
            if biErrorFlags.MotorDirectionFalse then
              //value for motor poles at axis is invalid
              LogError("@05FB (MDD2000_DriveAxisDiag::CyWork) The motor direction is wrong!");
              MotorPhasesOK := -2; 
            end_if;                        
            
            if biErrorFlags.MotorPolesUnallowedValue then
              HWC_LogPrintf(pThis:=this, format:= "@05F8 (MDD2000_DriveAxisDiag::CyWork) The number of measured motor poles ( %u )  does not give a valid value.", #MotorPolesMeasured);
              MotorPolesOK := -2;
            end_if;          
            
            if biErrorFlags.DiagNotSupportedFW then
              //angle mode -6 is not supported - new drive firmware is needed
              LogError("@05B7 (MDD2000_DriveAxisDiag::CyWork) G-Mode -6 is not supported (a newer drive firmware is needed).");
              MotorPolesOK  := -4;
              MotorPhasesOK :=  0;
            end_if;                
            
            if biErrorFlags.AxErrorNotQuitable then
              //value for motor poles at axis is invalid
              LogError("@05B8 (MDD2000_DriveAxisDiag::CyWork) There is an error on the axis that cannot be acknowledged. The error is displayed on the AxisState server.");
              MotorPolesOK  := -5;
              MotorPhasesOK :=  0;
            end_if;    
            
            biErrorFlags  := 0;
          end_if;
          
          //reset axis parameters to old values
          sDiagSSW := _SetAxisPara;
          
        elsif dRetCode = ERROR then
          MotorPolesOK   := -3;
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
          
        //stop actual diag  
        elsif  bStopDiag = TRUE & sParaSDOSSW = _IdlePara then
          MotorPolesOK  := 0;
          MotorPhasesOK := 0;
          //reset axis parameters to old values
          sDiagSSW := _SetAxisPara;        
        end_if;
        
        
      //**************************************************************
      teSSWDiag::_DiagGetFeedbackOffset:
        dRetCode := GetFeedbackOffset();
        if dRetCode = READY then
          dMoveAxisAngleCnt += 1;
          dActAngleDec      := 0;
                   
          if dMoveAxisAngleCnt < dPoleMeasurePoints then
            sDiagSSW := _DiagMoveAxis;
            return;
          else
            sDiagSSW := _SetAxisPara; 
            FeedbackOffsetOK := 1;       
          end_if;
          
          //check for occurred errors according to the choosen start diag mode
          if biErrorFlags.FeedbackMeasureLimits2 | biErrorFlags.FeedbackMeasureLimits1 then
            FeedbackOffsetOK := -1;
          //measured feedback offset and feedback offset on axis are different  
          elsif biErrorFlags.FeedbackOffsetOnAxis then
            FeedbackOffsetOK := 2;
          end_if;
            
          if biErrorFlags.MotorPolesOnAxisZero then
            FeedbackOffsetOK := -3;
            //value for motor poles at axis is invalid
            LogError("@05F9 (MDD2000_DriveAxisDiag::CyWork) Motorpoles of axis is set to zero which is no valid value!");
          end_if;
          
          if biErrorFlags.AxErrorNotQuitable then
            //the axis is in a not quitable error state
            LogError("@05B8 (MDD2000_DriveAxisDiag::CyWork) There is an error on the axis that cannot be acknowledged. The error is displayed on the AxisState server.");
            FeedbackOffsetOK := -6;
          end_if;
          
          if biErrorFlags.MotorDirectionFalse then
            //value for motor poles at axis is invalid
            LogError("@05FB (MDD2000_DriveAxisDiag::CyWork) The motor direction is wrong!");
            FeedbackOffsetOK := -8;
          end_if;
          
          biErrorFlags := 0;
          
        elsif dRetCode = ERROR then
          FeedbackOffsetOK := -2;
          sDiagErrorStep   := sDiagSSW;
          sDiagSSW         := _ErrorDiag; 
          
        //stop actual diag  
        elsif bStopDiag = TRUE & sParaSDOSSW = _IdlePara then
          FeedbackOffsetOK  := 0;
          //reset axis parameters to old values
          sDiagSSW := _SetAxisPara; 
        end_if;
        
      
      //**************************************************************
      teSSWDiag::_DiagMoveAxis:
        //movement is started at angle = 0
        dRetCode := MoveAxisAngle(dAngle:=0);
        if dRetCode = READY then
          sDiagSSW := _SetAxisPara;
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
          
        //stop actual diag  
        elsif  bStopDiag = TRUE & sParaSDOSSW = _IdlePara then
          //reset axis parameters to old values
          sDiagSSW := _SetAxisPara;          
        end_if;
              
              
      //***********************************************************************************
      teSSWDiag::_SetAxisPara:
        //look if old values must be written back to axis
        if bdSetOldValues then        
          dRetCode := SetAxisPara();
        else
          dRetCode := READY;
        end_if;

        if dRetCode = READY then
          bdSetOldValues := 0;
          
          if StartDiag = _GetAmountOfMotorPoles | bStopDiag = TRUE then
            sDiagSSW := _FinishedDiag;
            
          elsif StartDiag = _GetFeedbackOffset then 
            if biErrorFlags = 0 then            
              if dMoveAxisAngleCnt < dPoleMeasurePoints then
                //need more runs to finish
                sDiagSSW := _DiagGetFeedbackOffset;
              else
                sDiagSSW := _FinishedDiag;
              end_if;
            else              
              if biErrorFlags.DiagNotSupportedFW = TRUE then
                FeedbackOffsetOK := -4;
                LogError("@05FC (MDD2000_DriveAxisDiag::CyWork) Parameter I-FBOFF for feedback determinaton is not supported (a newer drive firmware is needed).");
              end_if;
              
              if biErrorFlags.MotorPolesPhaseError = TRUE then
                FeedbackOffsetOK := -5;
                LogError("@05F6 (MDD2000_DriveAxisDiag::CyWork)Motor phase or feedback is not ok (missing phase or feedback failure)!");
              end_if;
                  
              if biErrorFlags.AxErrorNotQuitable then
                //value for motor poles at axis is invalid
                LogError("@05B8 (MDD2000_DriveAxisDiag::CyWork) There is an error on the axis that cannot be acknowledged. The error is displayed on the AxisState server.");
                FeedbackOffsetOK := -6;
              end_if;
              
              if biErrorFlags.MotorDirectionFalse then
                //value for motor poles at axis is invalid
                LogError("@05FB (MDD2000_DriveAxisDiag::CyWork) The motor direction is wrong!");
                FeedbackOffsetOK := -8;
              end_if;
              
              //reset error flags
              biErrorFlags := 0;
              sDiagSSW := _FinishedDiag;
            end_if;                
          else
            sDiagSSW := _FinishedDiag;
          end_if;
          
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;
        
                 
      //***********************************************************************************
      teSSWDiag::_SetMPOLPara:
        //set M-POL
        dRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_POL, ParamValue:=MotorPolesMeasured);      
        if dRetCode = READY then
          if MotorPolesOK = 2 then
            MotorPolesOK := 1;
          end_if;
          //refresh axis parameters
          sDiagSSW := _ReadBackAxisPara;    
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;
                 
          
      //***********************************************************************************
      teSSWDiag::_SetMROFFPara:
        //set M-ROFF
        dRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_ROFF, ParamValue:=FeedbackOffsetMeasured);
        if dRetCode = READY then
          if FeedbackOffsetOK = 2 then
            FeedbackOffsetOK := 1;
          end_if;

          //refresh axis parameters
          sDiagSSW := _ReadBackAxisPara;    
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;
                 
        
      //***********************************************************************************
      teSSWDiag::_QuitAxisError:
        //quit axis error
        dRetCode := QuitAxError();
        if dRetCode = READY then
          //finish quit error
          sDiagSSW := _FinishedDiag;    
        elsif dRetCode = ERROR then
          sDiagErrorStep := sDiagSSW;
          sDiagSSW       := _ErrorDiag;
        end_if;               
        
        
      //**************************************************************
      teSSWDiag::_FinishedDiag:
        //dont unlock _DriveAxis if an error occurred
        if (MotorPolesOK = 0 | MotorPolesOK = 1) 
        & (MotorPhasesOK = 0 | MotorPhasesOK = 1)
        & (FeedbackOffsetOK = 0 | FeedbackOffsetOK = 1)      
        & bDiagModeStarted & QuitError = 0 then
          //unlock drive axis for other accesses
          SetDiagMode(bStartDiagMode:=FALSE, this);
          bDiagModeStarted := FALSE;
          bStopDiag        := FALSE;
          StopDiag         := 0;
        end_if;
        
        //reset quit error if drive type is fetched or new fw version is needed.
        if bGetDriveType = TRUE 
        | bsSupportedDiagMode.FeedbackOffset = FALSE
        | bsSupportedDiagMode.MotorPoles = FALSE then        
          QuitError := 0;
        end_if;

        sDiagSSW  := _IdleDiag;
        StartDiag := _IdleStartDiag;
      
      
      //*************************************************************
      teSSWDiag::_ErrorDiag: 
        if bdSetOldValues then        
          dRetCode := SetAxisPara();
          if dRetCode = READY | dRetCode = ERROR then  
            bdSetOldValues := 0;
          end_if;  
  //      else   
  //      //dont unlock _DriveAxis if an error occurred
  //        if bDiagModeStarted then
  //          //unlock drive axis for other accesses
  //          DriveIF.SetDiagMode(bStartDiagMode:=FALSE, this);
  //          bDiagModeStarted := FALSE;
  //        end_if;
        end_if;
        //reset start diag state
        StartDiag := _IdleStartDiag;      
        
        
      //**************************************************************
      teSSWDiag::_AxisNotOnline:
        DiagState := _ErrorDiag;
        QuitError.Write(input:=1);
        
    end_case;
  else    
    sDiagSSW  := _AxisNotOnline;
  end_if;  
  
  DiagState := sDiagSSW;
	state := READY;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::WriteParameter
	VAR_INPUT
		ParaID 	: DINT;
		ParaSubID 	: DINT;
		ParamValue 	: DINT;
		EntryType 	: USINT;(* := 0 *)
	END_VAR
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  
  dRetCode := BUSY;
  
  case sParaSDOSSW of
    //**************************************************************
  	teSSWSDOHandling::_IdlePara:
      sParaSDOSSW := _RdWrPara;
    
    //**************************************************************
  	teSSWSDOHandling::_RdWrPara:
    
      AddAsyncEntryDS402( ParaID    :=  ParaID$HINT
                        , ParaSubID :=  ParaSubID$HSINT
                        , ParaRdWr  :=  _DRIVEAXISDIAG_WRITEPARAMETER
                        , ParaSize  :=  MDD2000MNG_DATASIZE_4BYTE
                        , pParaValue:=  #ParamValue
                        , pThis     :=  this
                        , usEntry   :=  EntryType
                        , udTimeout :=  0
                        , dCallID   :=  0);
    
      sParaSDOSSW := _Wait4Data;
    
    //**************************************************************
  	teSSWSDOHandling::_Wait4Data:
      if State = READY then
        dRetCode    := READY;
        sParaSDOSSW := _IdlePara;
      end_if;    
    
  end_case;
  
  if State = ERROR then
    ErrorQuit.Write(input:=1);
    sParaSDOSSW := _IdlePara;
    dRetCode := ERROR;
  end_if;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::ReadParameter
	VAR_INPUT
		ParaID 	: DINT;
		ParaSubID 	: DINT;
		pParamValue 	: ^DINT;
		EntryType 	: USINT;(* := 0 *)
	END_VAR
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR

  dRetCode := BUSY;
  
  case sParaSDOSSW of
    //**************************************************************
  	teSSWSDOHandling::_IdlePara:
      sParaSDOSSW := _RdWrPara;
    
    //**************************************************************
  	teSSWSDOHandling::_RdWrPara:
    
      AddAsyncEntryDS402( ParaID    :=  ParaID$HINT
                        , ParaSubID :=  ParaSubID$HSINT
                        , ParaRdWr  :=  _DRIVEAXISDIAG_READPARAMETER
                        , ParaSize  :=  MDD2000MNG_DATASIZE_4BYTE
                        , pParaValue:=  NIL
                        , pThis     :=  this
                        , usEntry   :=  EntryType
                        , udTimeout :=  0
                        , dCallID   :=  0);
    
      sParaSDOSSW := _Wait4Data;
    
    //**************************************************************
  	teSSWSDOHandling::_Wait4Data:
      if State = READY then
        pParamValue^ := dParaValue;
        dRetCode := READY;
        sParaSDOSSW := _IdlePara;
      end_if;    
    
  end_case;  
  
  if State = ERROR then
    ErrorQuit.Write(input:=1);
    sParaSDOSSW := _IdlePara;
    dRetCode := ERROR;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::StartDiag::Write
	VAR_INPUT
		input (EAX) 	: MDD2000_DriveAxisDiag::teSSWDiagControl;
	END_VAR
	VAR_OUTPUT
		result (EAX) 	: MDD2000_DriveAxisDiag::teSSWDiagControl;
	END_VAR
  VAR
  	dRetCode : DINT;
    bTmpDiagMode : BOOL;
    bChangeState : BOOL;
  END_VAR
  
  if StartDiag = MDD2000_DriveAxisDiag::teSSWDiagControl::_IdleStartDiag & IsAxisOnline() then
    bTmpDiagMode := bDiagModeStarted;    
    bChangeState := FALSE;
    case input of
      //********************************************************************************
    	MDD2000_DriveAxisDiag::teSSWDiagControl::_GetAmountOfMotorPoles:      
        //reset values
        MotorPolesOK        := 0;
        MotorPhasesOK       := 0;
        MotorPolesMeasured  := 0;
        DirectionMeasured   := 0;
                
        //get max allowed position deviation
        if MotorPolesOnAxis then
          dMaxPositionDeviation := _DRIVEAXISDIAG_STEP_DEGREE_MECH/(MotorPolesOnAxis*2);
        else
          //defaultvalue for max position deviation
          dMaxPositionDeviation := _DRIVEAXISDIAG_STEP_DEGREE_MECH/20;
        end_if;
        
        bDiagModeStarted := TRUE;
        bChangeState     := TRUE;
        sDiagSSW := MDD2000_DriveAxisDiag::teSSWDiag::_ReadAxisPara;
        
      //********************************************************************************
    	MDD2000_DriveAxisDiag::teSSWDiagControl::_GetFeedbackOffset:
        bDiagModeStarted := TRUE;
        bChangeState     := TRUE;
        
        //reset valid flag for measure of feedback offset
        bsFeedbackValidFlag := 0;
        //reset feedback offset measure state
        FeedbackOffsetOK    := 0;        
        sDiagSSW := MDD2000_DriveAxisDiag::teSSWDiag::_ReadAxisPara;  
        
      //********************************************************************************
    	MDD2000_DriveAxisDiag::teSSWDiagControl::_SetMeasuredMotorPolesAtAxis:
        //only write value to axis if we have a valid measurement
        if MotorPolesOK > 0 then  
          bDiagModeStarted := TRUE;
          bChangeState     := TRUE;
          //set measured value to MotorPolesOnAxis
          MotorPolesOnAxis.Write(input:=MotorPolesMeasured);    
          sDiagSSW := MDD2000_DriveAxisDiag::teSSWDiag::_ReadAxisPara;   
        end_if;
        
      //********************************************************************************
    	MDD2000_DriveAxisDiag::teSSWDiagControl::_SetMeasuredFeedbackOffsetAtAxis:
        //only write value to axis if we have a valid measurement
        if FeedbackOffsetOK > 0 & bsFeedbackValidFlag.MeasuredFeedback1 = TRUE
        & MotorPolesOK >= 0 & MotorPolesOK < 2 then
          bDiagModeStarted := TRUE;
          bChangeState     := TRUE;
          sDiagSSW         := MDD2000_DriveAxisDiag::teSSWDiag::_ReadAxisPara;   
        end_if;        
        
      //********************************************************************************
    	MDD2000_DriveAxisDiag::teSSWDiagControl::_IdleStartDiag:      
        //do nothing if _idle step is selected
        return;
      
    end_case;
    
    if bDiagModeStarted & bChangeState then
      sMotorDiagSSW := _MotorDiagIdle;
      
      //initial retcode with ok
      dRetCode := 1;
      
      //if an error occurred at last test, the _DriveAxis is already locked
      if bTmpDiagMode = FALSE then
        dRetCode := SetDiagMode(bStartDiagMode:=bDiagModeStarted, this);
      end_if;
      
      if dRetCode = 1 then
        //diag mode has started 
        StartDiag := input;
      else
        //start of diag mode is not available because drive axis is not ready
        bDiagModeStarted := FALSE;
        sDiagSSW         := _IdleDiag;
        StartDiag        := _IdleStartDiag;        
      end_if; 
    else
      if bChangeState = FALSE then
        //change doesn't work
        sDiagSSW := _ErrorStartDiag;
      end_if;
    end_if;
  end_if;
  
  result := StartDiag;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::GetMotorPoles
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR
    udTmpRest : UDINT;
    udTempPolePairs : UDINT;
  	dTmpRetCode : iprStates;
  END_VAR
  
  dRetCode := BUSY;
  
  case sMotorDiagSSW of
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagIdle:
      //set initvalues
      dTmpDirection       := -1;      
      sPositionInfo.usPosChangeErrorCnt := 0;
      sPositionInfo.usDirectionErrorCnt := 0;
      bFinishedPoleMeasure := FALSE;
      //quit ax error if there is one
      if AxisErrorState <> 0 then
        sMotorDiagSSW := _Set_K_CLRF;
      else
        sMotorDiagSSW := _Set_A_ACME;
      end_if;
      
        
    //***********************************************************************************
  	teSSWMotorDiag::_Set_K_CLRF:
      //quit ax error      
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CLRF, ParamValue:=1);
      if dTmpRetCode = READY then
        sMotorDiagSSW := _Get_I_STATUS;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      

    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_STATUS:
      //get axis state
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        if AxisErrorState = 0 then        
          sMotorDiagSSW := _Set_A_ACME;
        else
          sGetMPolesErrorStep             := sMotorDiagSSW;
          biErrorFlags.AxErrorNotQuitable := TRUE;
          sMotorDiagSSW                   := _AxisDisable;          
        end_if;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Set_A_ACME:
      //don't save this parameter
      //set A-ACME Bit 5 to deactivate auto commute search
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_A_CME, ParamValue:=2#10000, EntryType := 2);
      if dTmpRetCode = READY then
        bdSetOldValues.P173_A_ACME := TRUE;
        sMotorDiagSSW := _Set_G_Mode;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep                  := sMotorDiagSSW;
        sMotorDiagSSW                        := _AxisDisable;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Set_G_Mode:
      //set G-Mode for diag
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_G_MODE, ParamValue:=_DRIVEAXISDIAG_ANGLE_SERVICEMODE, EntryType := 2);
      if dTmpRetCode = READY then
        bdSetOldValues.P032_G_MODE := TRUE;
        sMotorDiagSSW := _SetStartCurrent;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep                  := sMotorDiagSSW;
        biErrorFlags.DiagNotSupportedFW      := TRUE;
        sMotorDiagSSW                        := _AxisDisable;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetStartCurrent:
      dRampCurrent := 0;
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=(dRampCurrent));
      if dTmpRetCode = READY then
        dRampCurrentOneStep := (UsedCurrent*_DRIVEAXISDIAG_CURRENTSTEP_PERCENT)/100;
        dRampCurrent += dRampCurrentOneStep;
        bdSetOldValues.P226_K_CI_Reset := TRUE;
        sMotorDiagSSW     := _SetStartAngle;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;  
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetStartAngle:
      dActAngleDec := 0;
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_PHI, ParamValue:=dActAngleDec);
      if dTmpRetCode = READY then
        bdSetOldValues.P232_K_PHI_Reset := TRUE;
        sMotorDiagSSW := _AxisEnable;  
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
    
      
    //***********************************************************************************
  	teSSWMotorDiag::_AxisEnable:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=1);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := TRUE;
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW := _Wait4AxisEnable;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4AxisEnable:
      if (ops.tAbsolute-udStartWaitTime) >= (_DRIVEAXISDIAG_WAITTIME_AXENABLE) then
        sMotorDiagSSW := _SetRampCurrent;
      end_if;          
            
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetRampCurrent:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=dRampCurrent);
      if dTmpRetCode = READY then        
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW := _Wait4SetRampCurrent;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4SetRampCurrent:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_RAMPCURRENT then        
        dRampCurrent += dRampCurrentOneStep;
        if dRampCurrent <= UsedCurrent then
          sMotorDiagSSW := _SetRampCurrent;  
        else 
          udStartWaitTime := ops.tAbsolute;
          sMotorDiagSSW := _Get_I_FPOS_Start;
        end_if;
      end_if;      
          
    
    //***********************************************************************
  	teSSWMotorDiag::_Get_I_FPOS_Start:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_GETIFPOS_START then  
        dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_FPOS, pParamValue:=#sPositionInfo.dStartPos);
        if dTmpRetCode = READY then
          dActAngleDec += _DRIVEAXISDIAG_ANGLE_STEPS_DEC;
          sMotorDiagSSW  := _SetElectricAngle;
        elsif dTmpRetCode = ERROR then
          sGetMPolesErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;  
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetElectricAngle:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_PHI, ParamValue:=dActAngleDec);
      if dTmpRetCode = READY then
        // only increment act angle if we are not at last step
        if bFinishedPoleMeasure = FALSE then
          dActAngleDec   += _DRIVEAXISDIAG_ANGLE_STEPS_DEC;
        end_if;
        sMotorDiagSSW  := _Wait4SetAngle; 
        udStartWaitTime := ops.tAbsolute;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4SetAngle:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_SETANGLE then 
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW   := _Get_I_FPOS_End; 
      end_if;
      
    
    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_FPOS_End:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_GETIFPOS_END then  
        dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_FPOS, pParamValue:=#sPositionInfo.dEndPos);
        if dTmpRetCode = READY then
          if dTmpDirection < 0 then
            sPositionInfo.dLastPos := sPositionInfo.dStartPos;
          end_if;  
          sMotorDiagSSW   := _CheckValues;                   
        elsif dTmpRetCode = ERROR then
          sGetMPolesErrorStep := sMotorDiagSSW;
          sMotorDiagSSW       := _MotorDiagError;
        end_if;
      end_if;  
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_CheckValues:      
      if dActAngleDec < _DRIVEAXISDIAG_MAX_ANGLE_DEC & bFinishedPoleMeasure = FALSE then
        if dActAngleDec > (_DRIVEAXISDIAG_MAX_ANGLE_DEC-(_DRIVEAXISDIAG_ANGLE_STEPS_DEC/2)) then
          //set to start position 0°
          dActAngleDec := 0;
          bFinishedPoleMeasure := TRUE;
        end_if;
        sMotorDiagSSW  := _SetElectricAngle;
      else
        sMotorDiagSSW := _AxisDisable;
      end_if;
          
      sPositionInfo.dTmpDiffPos := sPositionInfo.dEndPos-sPositionInfo.dLastPos;
      if dTmpDirection < 0 then
        //DirectionMeasured of position move
        if sPositionInfo.dTmpDiffPos > 0 then
          dTmpDirection  := 1;
          sPositionInfo.usDirectionErrorCnt := 0;          
        else // wrong direction
          dTmpDirection  := 0;
          sPositionInfo.usDirectionErrorCnt += 1;
          if sPositionInfo.usDirectionErrorCnt > _DRIVEAXISDIAG_WRONG_POSITION_RETRYS then
            biErrorFlags.MotorDirectionFalse := TRUE;
          end_if;
        end_if;
      end_if;
      
      //position has not changed -> motor phase error
      if abs(sPositionInfo.dTmpDiffPos) < dMaxPositionDeviation then
        sPositionInfo.usPosChangeErrorCnt += 1;
        if sPositionInfo.usPosChangeErrorCnt > _DRIVEAXISDIAG_WRONG_POSITION_RETRYS then
          biErrorFlags.MotorPolesPhaseError := TRUE;
        end_if;
      else 
        sPositionInfo.usPosChangeErrorCnt := 0;
      end_if;
      
      sPositionInfo.dLastPos := sPositionInfo.dEndPos;
      
      //finish motor pole determining if an error occured
      if biErrorFlags then
        sMotorDiagSSW  := _AxisDisable;
      end_if;
          
    
    //***********************************************************************************
  	teSSWMotorDiag::_AxisDisable:    
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_AXENABLE then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0);
        if dTmpRetCode = READY then
          bdSetOldValues.P220_K_EN_AxDisable := FALSE;
          sMotorDiagSSW := _CheckValues2;
        elsif dTmpRetCode = ERROR then
          sGetMPolesErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_CheckValues2: 
      if biErrorFlags = 0 then
        sPositionInfo.dTmpDiffPos := sPositionInfo.dEndPos-sPositionInfo.dStartPos;
        //DirectionMeasured of position move
        if sPositionInfo.dTmpDiffPos > 0 then
          DirectionMeasured  := 1;
        else
          DirectionMeasured  := 0;
          biErrorFlags.MotorDirectionFalse := TRUE;
        end_if;
        
        sPositionInfo.udDiffPos := to_udint(abs(sPositionInfo.dTmpDiffPos));
        udTempPolePairs         := _DRIVEAXISDIAG_ONE_TURN_HARDWARE/sPositionInfo.udDiffPos;
        udTmpRest               := _DRIVEAXISDIAG_ONE_TURN_HARDWARE mod (sPositionInfo.udDiffPos);
        //add one pole pair if diff position is near it
        if udTmpRest > (_DRIVEAXISDIAG_ONE_TURN_HARDWARE/udTempPolePairs)/2 then
          udTempPolePairs += 1;          
        end_if;
        
        //convert udTempPolePairs to Motorpoles measured
        MotorPolesMeasured := (udTempPolePairs$DINT*4)/(abs(sAxisParameter.dP013_M_RPOL));
        if MotorPolesMeasured <> MotorPolesOnAxis then 
          biErrorFlags.MotorPolesToAxisDiff := TRUE;
        end_if;
        
        if abs(MotorPolesMeasured) > _DRIVEAXISDIAG_MAX_MOTOR_POLES 
        | abs(MotorPolesMeasured) < _DRIVEAXISDIAG_MIN_MOTOR_POLES
        | MotorPolesMeasured mod 2 <> 0 then
          biErrorFlags.MotorPolesUnallowedValue := TRUE;
        end_if;

      end_if;  
      sMotorDiagSSW  := _MotorDiagFinished;     
        
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagFinished:
      dRetCode       := READY;    
      sMotorDiagSSW  := _MotorDiagIdle;
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagError:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_AXENABLE then
        //disable axis
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0);
        if dTmpRetCode = READY then
          dRetCode  := ERROR;
        end_if;
      end_if;    
    
  end_case;  
  
END_FUNCTION

FUNCTION MDD2000_DriveAxisDiag::GetFeedbackOffset
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR
  	dTmpRetCode : iprStates;
    dTmpValue   : DINT;
    dTmpROFF : DINT;
    dMinVal : DINT;
    dMaxVal : DINT;
    dTmpDiff : DINT;
  END_VAR
  
  dRetCode := BUSY;
  
  case sMotorDiagSSW of
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagIdle:      
      //quit ax error if there is one
      if AxisErrorState <> 0 then
        sMotorDiagSSW := _Set_K_CLRF;
      else
        sMotorDiagSSW := _Set_A_ACME;
      end_if;
      
        
    //***********************************************************************************
  	teSSWMotorDiag::_Set_K_CLRF:
      //quit ax error
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CLRF, ParamValue:=1);
      if dTmpRetCode = READY then
        sMotorDiagSSW := _Get_I_STATUS;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      

    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_STATUS:
      //read ax state
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        if AxisErrorState = 0 then        
          sMotorDiagSSW := _Set_A_ACME;
        else
          sGetMPolesErrorStep             := sMotorDiagSSW;
          biErrorFlags.AxErrorNotQuitable := TRUE;
          sMotorDiagSSW                   := _AxisDisable;          
        end_if;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      
    
    //***********************************************************************************
  	teSSWMotorDiag::_Set_A_ACME:
      //set A-ACME Bit 5 to deactivate auto commute search
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_A_CME, ParamValue:=2#10000, EntryType := 2);

      if dTmpRetCode = READY then
        bdSetOldValues.P173_A_ACME := TRUE;
        sMotorDiagSSW := _Set_G_Mode;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Set_G_Mode:
      //set G-Mode for diag
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_G_MODE, ParamValue:=_DRIVEAXISDIAG_ANGLE_SERVICEMODE, EntryType := 2);
      
      if dTmpRetCode = READY then
        bdSetOldValues.P032_G_MODE := TRUE;
        sMotorDiagSSW := _SetStartCurrent;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetStartCurrent:
      //set current from 0 to 100%
      dRampCurrent := 0;
      dTmpRetCode  := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=dRampCurrent);
      
      if dTmpRetCode = READY then
        bdSetOldValues.P226_K_CI_Reset := TRUE;
        dRampCurrentOneStep := (UsedCurrent*_DRIVEAXISDIAG_CURRENTSTEP_PERCENT)/100;
        dRampCurrent   += dRampCurrentOneStep;
        sMotorDiagSSW  := _AxisEnable;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;  
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_AxisEnable:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=1);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := TRUE;
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW := _Wait4AxisEnable;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4AxisEnable:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_AXENABLE then
        sMotorDiagSSW := _SetRampCurrent;  
      end_if; 
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetRampCurrent:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=dRampCurrent);
      if dTmpRetCode = READY then
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW := _Wait4SetRampCurrent;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4SetRampCurrent:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_RAMPCURRENT then        
        dRampCurrent += dRampCurrentOneStep;
        if dRampCurrent <= UsedCurrent then
          sMotorDiagSSW := _SetRampCurrent;  
        else
          udStartWaitTime := ops.tAbsolute;
          sMotorDiagSSW := _Get_I_FPOS_Start;
        end_if;
      end_if;      
          
    
    //***********************************************************************
  	teSSWMotorDiag::_Get_I_FPOS_Start:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_GETIFPOS_START then    
        dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_FPOS, pParamValue:=#dTmpValue);
        if dTmpRetCode = READY then
          sPositionInfo.dStartPos := dTmpValue;
          udStartWaitTime := ops.tAbsolute;
          sMotorDiagSSW  := _Get_I_FBOFF;
        elsif dTmpRetCode = ERROR then
          sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;  
      
    
    //***********************************************************************
  	teSSWMotorDiag::_Get_I_FBOFF:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_COMMAND then    
        dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_MPOS, pParamValue:=#dTmpValue);
        if dTmpRetCode = READY then
          aFeedbackOffset[dMoveAxisAngleCnt].dI_FBOFF := dTmpValue;
          udStartWaitTime := ops.tAbsolute;
          sMotorDiagSSW  := _AxisDisable;
        elsif dTmpRetCode = ERROR then
          sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_AxisDisable:    
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := FALSE;
        sMotorDiagSSW := _CheckValues;
      elsif dTmpRetCode = ERROR then
        sGetFeedbackOffsetErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_CheckValues:
      //set motorpoles measured value for calculation if it is valid
      if MotorPolesMeasured & MotorPolesOK > 0 then
        sAxisParameter.dP007_M_POL := MotorPolesMeasured;
      end_if;
    
      if sAxisParameter.dP007_M_POL = 0 then
        //amount of zero motorpoles is not valid
        dMoveAxisAngleCnt := dPoleMeasurePoints-1;
        biErrorFlags.MotorPolesOnAxisZero := TRUE;
      end_if;
      
      //check for last run to check measured values
      if dMoveAxisAngleCnt = dPoleMeasurePoints-1 then
        sMotorDiagSSW := _CheckValues2;
      else
        sMotorDiagSSW := _MotorDiagFinished;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_CheckValues2:
    
      #if _DRIVEAXISDIAG_FEEDBACK_MEASURE_POINTS <> 2
        // if more the 2 values should be considered, an other algorythm needs to be developed.
        #pragma message("ERROR: only 2 measure points are support for mean value calculation")
      #endif
    
      // calculate the mean with overflow at 360
      if aFeedbackOffset[0].dI_FBOFF > aFeedbackOffset[1].dI_FBOFF then
        dMinVal := aFeedbackOffset[1].dI_FBOFF;
        dMaxVal := aFeedbackOffset[0].dI_FBOFF;
      else
        dMinVal := aFeedbackOffset[0].dI_FBOFF;
        dMaxVal := aFeedbackOffset[1].dI_FBOFF;
      end_if;
      dTmpDiff := dMaxVal - dMinVal;
      if (dTmpDiff) > 180 then
        dTmpDiff := 360 - dTmpDiff;
        dTmpROFF := (360 + ((dMinVal + (dMaxVal - 360)) / 2)) MOD 360;
      else
        dTmpROFF := (360 + ((dMinVal + dMaxVal) / 2)) MOD 360;
      end_if;

      // check if the two measured values are within range
      if  dTmpDiff > dFeedbackMaxElectricDeviation then
        HWC_LogPrintf(pThis:=this, format:= "@05B9 (MDD2000_DriveAxisDiag::GetFeedbackOffset) Measured feedback offset ( %u ) is not in valid range ( %u ) to measured offset2 ( %u ).",#aFeedbackOffset[0].dI_FBOFF, #dFeedbackMaxElectricDeviation, #aFeedbackOffset[1].dI_FBOFF);
        //set zero Value on server
        FeedbackOffsetMeasured := 0;
        biErrorFlags.FeedbackMeasureLimits1 := TRUE;
      else
        //sanity check, should be impossible because of MOD 360
        if dTmpROFF >= 0 & dTmpROFF < _DRIVEAXISDIAG_MAX_ANGLE_DEGREE then
          // a new valid M-ROFF has been measured
          bsFeedbackValidFlag.MeasuredFeedback1 := TRUE;
          FeedbackOffsetMeasured := dTmpROFF;

          //check if measured feedback offset is within the limits to feedback offset of axis
          if FeedbackOffsetMeasured > FeedbackOffsetOnAxis then
            dMinVal := FeedbackOffsetOnAxis;
            dMaxVal := FeedbackOffsetMeasured;
          else
            dMinVal := FeedbackOffsetMeasured;
            dMaxVal := FeedbackOffsetOnAxis;
          end_if;
          dTmpDiff := dMaxVal - dMinVal;
          if (dTmpDiff) > 180 then
            dTmpDiff := 360 - dTmpDiff;
          end_if;
          if dTmpDiff > dFeedbackMaxElectricDeviation then
           HWC_LogPrintf(pThis:=this, format:= "@05BA (MDD2000_DriveAxisDiag::GetFeedbackOffset) Measured feedback offset ( %u ) is not in valid range ( %u ) with M-ROFF on axis ( %u )." ,#FeedbackOffsetMeasured, #dFeedbackMaxElectricDeviation, #FeedbackOffsetOnAxis);
           biErrorFlags.FeedbackOffsetOnAxis := TRUE;
          end_if;            
        else
          HWC_LogPrintf(pThis:=this, format:= "@05BB (MDD2000_DriveAxisDiag::GetFeedbackOffset) Measured value for feedback offset is not in valid range ( %u ).",#dTmpROFF);
          biErrorFlags.FeedbackMeasureLimits1 := TRUE;
        end_if;
      end_if;          
    
      sMotorDiagSSW := _MotorDiagFinished;    
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagFinished:
      dRetCode       := READY;    
      sMotorDiagSSW  := _MotorDiagIdle;
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagError:
      dRetCode  := ERROR;    
    
  end_case;
  
END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::MoveAxisAngle
	VAR_INPUT
		dAngle 	: DINT;
	END_VAR
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR
  	dTmpRetCode : iprStates;
  END_VAR

  dRetCode := BUSY;
  
  case sMotorDiagSSW of
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagIdle:
      //set initvalues
      sPositionInfo.usPosChangeErrorCnt := 0;
      sPositionInfo.usDirectionErrorCnt := 0;
      dTmpDirection := -_DRIVEAXISDIAG_ANGLE_STEPS_AMOUNT/2;
      bLastElecTurn := FALSE;
      //quit ax error if there is one
      if AxisErrorState <> 0 then
        sMotorDiagSSW := _Set_K_CLRF;
      else
        sMotorDiagSSW := _Set_A_ACME;
      end_if;
      
        
    //***********************************************************************************
  	teSSWMotorDiag::_Set_K_CLRF:
      //quit ax error
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CLRF, ParamValue:=1);
      if dTmpRetCode = READY then
        sMotorDiagSSW := _Get_I_STATUS;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      

    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_STATUS:
      //read state of drive
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        if AxisErrorState = 0 then        
          sMotorDiagSSW := _Set_A_ACME;
        else
          sMoveAxisAngleErrorStep         := sMotorDiagSSW;
          biErrorFlags.AxErrorNotQuitable := TRUE;
          sMotorDiagSSW                   := _AxisDisable;          
        end_if;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Set_A_ACME:
      //set A-ACME Bit 5 to deactivate auto commute search
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_A_CME, ParamValue:=2#10000, EntryType := 2);
      if dTmpRetCode = READY then
        bdSetOldValues.P173_A_ACME := TRUE;
        sMotorDiagSSW := _Set_G_Mode;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if; 
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Set_G_Mode:
      //set G-Mode for diag
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_G_MODE, ParamValue:=_DRIVEAXISDIAG_ANGLE_SERVICEMODE, EntryType := 2);
      if dTmpRetCode = READY then
        bdSetOldValues.P032_G_MODE := TRUE;
        sMotorDiagSSW := _SetStartCurrent;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        biErrorFlags.DiagNotSupportedFW := TRUE;
        sMotorDiagSSW  := _AxisDisable;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetStartCurrent:
      dRampCurrent := 0;
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=dRampCurrent);
      if dTmpRetCode = READY then
        bdSetOldValues.P226_K_CI_Reset := TRUE;
        sMotorDiagSSW                  := _SetStartAngle;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;  
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetStartAngle:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_PHI, ParamValue:=dAngle);
      if dTmpRetCode = READY then
        bdSetOldValues.P232_K_PHI_Reset := TRUE;
        sMotorDiagSSW := _AxisEnable;  
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
    
          
    //***********************************************************************************
  	teSSWMotorDiag::_AxisEnable:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=1);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := TRUE;
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW := _Wait4AxisEnable;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4AxisEnable:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_AXENABLE then
        sMotorDiagSSW := _SetRampCurrent;
      end_if;
      
      
    //***********************************************************************************
  	teSSWMotorDiag::_SetRampCurrent:
      // limit to half the current (seems to be better)
      if CurrentReduction < 50 then
        dRampCurrent := UsedCurrent;
      else
        dRampCurrent := UsedCurrent/2;
      end_if;
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=dRampCurrent);
      if dTmpRetCode = READY then
        bdSetOldValues.P226_K_CI_Reset := TRUE;
        sMotorDiagSSW                  := _SetElectricAngle;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;  
      
           
    //***********************************************************************************
  	teSSWMotorDiag::_SetElectricAngle:
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_PHI, ParamValue:=dActAngleDec);
      if dTmpRetCode = READY then;
        sMotorDiagSSW   := _Wait4SetAngle; 
        udStartWaitTime := ops.tAbsolute;
      elsif dTmpRetCode = ERROR then
        sMoveAxisAngleErrorStep := sMotorDiagSSW;
        sMotorDiagSSW := _MotorDiagError;
      end_if;
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_Wait4SetAngle:
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_SETANGLE then 
        udStartWaitTime := ops.tAbsolute;
        sMotorDiagSSW  := _Get_I_FPOS_End;
      end_if;
      
    
    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_FPOS_End: 
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_GETIFPOS_END then  
        dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_FPOS, pParamValue:=#sPositionInfo.dEndPos);
        if dTmpRetCode = READY then
          dActAngleDec  += _DRIVEAXISDIAG_ANGLE_STEPS_DEC;
          if dActAngleDec < _DRIVEAXISDIAG_MAX_ANGLE_DEC & bLastElecTurn = FALSE then
            if dActAngleDec > (_DRIVEAXISDIAG_MAX_ANGLE_DEC-(_DRIVEAXISDIAG_ANGLE_STEPS_DEC)) then
              dActAngleDec  := 0;
              bLastElecTurn := TRUE;
            end_if;          
            
            if dTmpDirection >= 0 then          
              //check movement              
              sPositionInfo.dTmpDiffPos := sPositionInfo.dEndPos-sPositionInfo.dLastPos;
              
              //position has not changed -> motor phase error
              if abs(sPositionInfo.dTmpDiffPos) < dMaxPositionDeviation then 
                sPositionInfo.usPosChangeErrorCnt += 1;
                if sPositionInfo.usPosChangeErrorCnt > _DRIVEAXISDIAG_WRONG_POSITION_RETRYS then
                  biErrorFlags.MotorPolesPhaseError := TRUE;
                end_if;
              else      
                sPositionInfo.usPosChangeErrorCnt := 0;
                //DirectionMeasured of position move
                if sPositionInfo.dTmpDiffPos > 0 then
                  dTmpDirection  := 1;
                  sPositionInfo.usDirectionErrorCnt := 0; 
                else  //wrong direction
                  dTmpDirection  := 0;
                  sPositionInfo.usDirectionErrorCnt += 1;
                  if sPositionInfo.usDirectionErrorCnt > _DRIVEAXISDIAG_WRONG_POSITION_RETRYS then
                    biErrorFlags.MotorDirectionFalse := TRUE;
                  end_if;
                end_if;  
              end_if;              
              sPositionInfo.dLastPos := sPositionInfo.dEndPos;
                        
            else
              dTmpDirection += 1;
              if dTmpDirection <= -_DRIVEAXISDIAG_ANGLE_STEPS_AMOUNT/2  then
                sPositionInfo.dStartPos := sPositionInfo.dEndPos;
              end_if;
            end_if; 
            
            //finish motor pole determining if an error occured
            if biErrorFlags = 0 then
              sMotorDiagSSW  := _SetElectricAngle;
            else
              sMotorDiagSSW  := _AxisDisable;
            end_if;         
            
          else
            dActAngleDec    := 0;    
            udStartWaitTime := ops.tAbsolute;
            sMotorDiagSSW   := _AxisDisable;  
          end_if;      
        elsif dTmpRetCode = ERROR then
          sGetMPolesErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;  
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_AxisDisable:    
      if (ops.tAbsolute-udStartWaitTime) >= _DRIVEAXISDIAG_WAITTIME_AXENABLE then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0); 
        if dTmpRetCode = READY then       
          bdSetOldValues.P220_K_EN_AxDisable := FALSE;
          sMotorDiagSSW := _MotorDiagFinished;
        elsif dTmpRetCode = ERROR then
          sMoveAxisAngleErrorStep := sMotorDiagSSW;
          sMotorDiagSSW := _MotorDiagError;
        end_if;
      end_if;

    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagFinished:
      dRetCode       := READY;    
      sMotorDiagSSW  := _MotorDiagIdle;
    
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagError:
      dRetCode  := ERROR;
    
  end_case;
  
  
END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::FeedbackMaxDeviation::Write
	VAR_INPUT
		input (EAX) 	: DINT;
	END_VAR
	VAR_OUTPUT
		result (EAX) 	: DINT;
	END_VAR
  
  if input > _DRIVEAXISDIAG_MAX_ANGLE_DEGREE then
    input := _DRIVEAXISDIAG_MAX_ANGLE_DEGREE;
  elsif input < 0 then
    input := 0;
  end_if;
  
	FeedbackMaxDeviation := input;
	result := FeedbackMaxDeviation;

END_FUNCTION


FUNCTION GLOBAL MDD2000_DriveAxisDiag::LogError
	VAR_INPUT
		pMsg 	: ^CHAR;
	END_VAR

#ifdef HWC_LogError
  HWC_LogError(this, pMsg);
#else
  if bLoggingNotSupported = FALSE then
    TRACE("Logging is not supported, HWControl version 4.13 or above is needed!");
    bLoggingNotSupported := TRUE;
  end_if;
#endif

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::QuitError::Write
	VAR_INPUT
		input (EAX) 	: DINT;
	END_VAR
	VAR_OUTPUT
		result (EAX) 	: DINT;
	END_VAR  
  
  if (DiagState = _ErrorDiag | DiagState = _ErrorStartDiag | DiagState = _NotSupported
  | MotorPolesOK < 0 | FeedbackOffsetOK < 0 | (AxisErrorState <> 0 & DiagState = _IdleDiag)) 
  & input then
    //reset diag class
    sDiagErrorStep              := _IdleDiag;
    sMotorDiagSSW               := _MotorDiagIdle;
    sGetMPolesErrorStep         := _MotorDiagIdle;
    sGetFeedbackOffsetErrorStep := _MotorDiagIdle;
    sMoveAxisAngleErrorStep     := _MotorDiagIdle;
    sParaAxisSSW                := _ParaIdle;
    sParaAxisErrorStep          := _ParaIdle;;
    bStopDiag                   := FALSE;
    bGetDriveType               := FALSE;
    StopDiag                    := 0;
    MotorPolesOK                := 0;
    MotorPhasesOK               := 0;
    MotorPolesMeasured          := 0;
    FeedbackOffsetOK            := 0;
    
    //quit error if axis is in an error state
    if AxisErrorState = 0 then
      sDiagSSW := _FinishedDiag;
    else
      sDiagSSW := _QuitAxisError;
    end_if;
    
    QuitError := input;
  	result := QuitError;
  end_if;  

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::GetAxisPara
	VAR_OUTPUT
		dRetcode 	: iprStates;
	END_VAR
  VAR
  	dTmpRetcode : iprStates;
  END_VAR

  dRetcode := BUSY;
  
  case sParaAxisSSW of
    //***********************************************************************************
  	teSSWParaAxis::_ParaIdle:
      sParaAxisSSW := _P220_K_EN_Reset; 
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P220_K_EN_Reset:    
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := FALSE;
        sParaAxisSSW := _P004_M_INULL;
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P004_M_INULL:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_INULL, pParamValue:=#sAxisParameter.dP004_M_INULL);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P186_I_ICONT;   
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;
      
    //***********************************************************************************
  	teSSWParaAxis::_P186_I_ICONT:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_ICONT, pParamValue:=#sAxisParameter.dP186_I_ICONT);
      if dTmpRetCode = READY then
      
        // calculate the current to use for the measurements
        if sAxisParameter.dP186_I_ICONT < sAxisParameter.dP004_M_INULL then
          UsedCurrent := sAxisParameter.dP186_I_ICONT * CurrentReduction / 100;
        else
          UsedCurrent := sAxisParameter.dP004_M_INULL * CurrentReduction / 100;
        end_if;
      
        sParaAxisSSW := _P007_M_POL;   
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;
      
    
    //***********************************************************************************
  	teSSWParaAxis::_P007_M_POL:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_POL, pParamValue:=#sAxisParameter.dP007_M_POL);
      if dTmpRetCode = READY then        
        MotorPolesOnAxis := sAxisParameter.dP007_M_POL;
        if MotorPolesOK = 1 
        & MotorPolesMeasured <> MotorPolesOnAxis then
          //change state of MotorPolesOk if the value is different to axis value.
          MotorPolesOK := 2;
        end_if;
        sParaAxisSSW := _P012_M_TYPE;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;      
    
    
    //***********************************************************************************
  	teSSWParaAxis::_P012_M_TYPE:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_TYPE, pParamValue:=#sAxisParameter.dP012_M_TYPE);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P013_M_RPOL;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;      
    
    
    //***********************************************************************************
  	teSSWParaAxis::_P013_M_RPOL:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_RPOL, pParamValue:=#sAxisParameter.dP013_M_RPOL);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P014_M_ROFF;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
    
    
    //***********************************************************************************
  	teSSWParaAxis::_P014_M_ROFF:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_ROFF, pParamValue:=#sAxisParameter.dP014_M_ROFF);
      if dTmpRetCode = READY then
        FeedbackOffsetOnAxis := sAxisParameter.dP014_M_ROFF;
        sParaAxisSSW := _P015_M_RPULSE;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
    
    
    //***********************************************************************************
  	teSSWParaAxis::_P015_M_RPULSE:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_M_RPULSE, pParamValue:=#sAxisParameter.dP015_M_RPULSE);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P032_G_MODE;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
    
    
    //***********************************************************************************
  	teSSWParaAxis::_P032_G_MODE:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_G_MODE, pParamValue:=#sAxisParameter.dP032_G_MODE);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P173_A_ACME;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P173_A_ACME:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_A_CME, pParamValue:=#sAxisParameter.dP173_A_ACME);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P180_I_FBOFF;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;    
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P180_I_FBOFF:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_MPOS, pParamValue:=#sAxisParameter.dP180_I_FBOFF);
      if dTmpRetCode = READY then
        sParaAxisSSW := _P199_I_STATUS;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;    
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P199_I_STATUS:
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        sParaAxisSSW := _ParaFinished;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
      
    //***********************************************************************************
  	teSSWParaAxis::_ParaFinished:      
      sParaAxisSSW := _ParaIdle; 
      dRetcode      := READY;
    
    
    //***********************************************************************************
  	teSSWParaAxis::_ParaError:
      sParaAxisSSW   := _ParaIdle; 
      dRetcode := ERROR;
    
  end_case;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::SetAxisPara
	VAR_OUTPUT
		dRetcode 	: iprStates;
	END_VAR
  VAR
  	dTmpRetcode : iprStates;
  END_VAR

  dRetcode := BUSY;
  
  case sParaAxisSSW of
    //***********************************************************************************
  	teSSWParaAxis::_ParaIdle:
      sParaAxisSSW := _P220_K_EN_Reset; 
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P220_K_EN_Reset:    
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_EN, ParamValue:=0);
      if dTmpRetCode = READY then
        bdSetOldValues.P220_K_EN_AxDisable := FALSE;
        sParaAxisSSW := _P032_G_MODE;
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;
      
    //***********************************************************************************
  	teSSWParaAxis::_P032_G_MODE:
      if bdSetOldValues.P032_G_MODE then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_G_MODE, ParamValue:=sAxisParameter.dP032_G_MODE);
      else
        dTmpRetcode := READY;
      end_if;
      
      if dTmpRetCode = READY then
        sParaAxisSSW := _P173_A_ACME;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P173_A_ACME:
      if bdSetOldValues.P173_A_ACME then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_SET , ParaSubID:= MDD2000_SPID_A_CME, ParamValue:=sAxisParameter.dP173_A_ACME);
      else
        dTmpRetcode := READY;
      end_if;
      
      if dTmpRetCode = READY then
        sParaAxisSSW := _P226_K_CI_Reset;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;          
      
         
    //***********************************************************************************
  	teSSWParaAxis::_P226_K_CI_Reset:
      if bdSetOldValues.P226_K_CI_Reset then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CI, ParamValue:=0);
      else
        dTmpRetcode := READY;
      end_if;
      
      if dTmpRetCode = READY then
        sParaAxisSSW := _P232_K_PHI_Reset;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;
      
             
    //***********************************************************************************
  	teSSWParaAxis::_P232_K_PHI_Reset:
      if bdSetOldValues.P232_K_PHI_Reset then
        dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_PHI, ParamValue:=0);
      else
        dTmpRetcode := READY;
      end_if;
      
      if dTmpRetCode = READY then
        sParaAxisSSW := _P199_I_STATUS;  
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;       
      
      
    //***********************************************************************************
  	teSSWParaAxis::_P199_I_STATUS:
      //read ax state
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        sParaAxisSSW := _ParaFinished;
      elsif dTmpRetCode = ERROR then
        sParaAxisErrorStep := sParaAxisSSW;
        sParaAxisSSW := _ParaError;
      end_if;           
      
      
    //***********************************************************************************
  	teSSWParaAxis::_ParaFinished: 
      sParaAxisSSW   := _ParaIdle; 
      dRetcode        := READY;
    
    
    //***********************************************************************************
  	teSSWParaAxis::_ParaError:
      sParaAxisSSW := _ParaIdle; 
      dRetcode      := ERROR;
    
  end_case;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::StopDiag::Write
	VAR_INPUT
		input (EAX) 	: DINT;
	END_VAR
	VAR_OUTPUT
		result (EAX) 	: DINT;
	END_VAR
  
  if DiagState <> _IdleDiag & DiagState <> _ErrorDiag & DiagState <> _ErrorStartDiag then

    StopDiag  := input;
    bStopDiag := TRUE;
    result    := StopDiag;
  
  end_if;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::QuitAxError
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR
  	dTmpRetCode : iprStates;
  END_VAR
  
  dRetCode := BUSY;
  
  case sMotorDiagSSW of
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagIdle:
      //quit ax error if there is one
      if AxisErrorState <> 0 then
        sMotorDiagSSW := _Set_K_CLRF;
      else     
        sMotorDiagSSW := _Get_I_STATUS;
      end_if;      
        
    //***********************************************************************************
  	teSSWMotorDiag::_Set_K_CLRF:
      //quit ax error
      dTmpRetCode := WriteParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_K_CLRF, ParamValue:=1);
      if dTmpRetCode = READY then
        sMotorDiagSSW := _Get_I_STATUS;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if;       

    //***********************************************************************************
  	teSSWMotorDiag::_Get_I_STATUS:
      //read out ax state
      dTmpRetCode := ReadParameter(ParaID:= MDD2000_PID_SIGMATEKPARA_INDEX_INFO , ParaSubID:= MDD2000_SPID_I_STATUS, pParamValue:=#sAxisParameter.dP199_I_STATUS);
      if dTmpRetCode = READY then
        AxisErrorState   := sAxisParameter.dP199_I_STATUS$BDINT;
        sMotorDiagSSW  := _MotorDiagFinished;
      elsif dTmpRetCode = ERROR then
        sGetMPolesErrorStep := sMotorDiagSSW;
        sMotorDiagSSW  := _AxisDisable;
      end_if;         
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagFinished:
      dRetCode       := READY;    
      sMotorDiagSSW  := _MotorDiagIdle;    
    
    //***********************************************************************************
  	teSSWMotorDiag::_MotorDiagError:
      dRetCode  := ERROR;
    
  end_case;

END_FUNCTION


FUNCTION VIRTUAL MDD2000_DriveAxisDiag::CheckFWVersion
	VAR_OUTPUT
		dRetCode 	: DINT;
	END_VAR
  
  //0 .. motor diagnosis not supported
  //1 .. Variant 1 supported
  //2 .. Variant 2 supported
  //3 .. booth variants suppported
  
  //for new drives it is assumed that these support both variants
  //_DRIVETYPE::_NotFound:
  dRetCode := 3;

END_FUNCTION

FUNCTION VIRTUAL GLOBAL MDD2000_DriveAxisDiag::CurrentReduction::Write
	VAR_INPUT
		input (EAX) 	: DINT;
	END_VAR
	VAR_OUTPUT
		result (EAX) 	: DINT;
	END_VAR

  // only 1 to 100% is valid
  if input > 100 then
    input := 100;
  elsif input < 1 then
    input := 1;
  end_if;

	CurrentReduction := input;
	result := CurrentReduction;

END_FUNCTION


FUNCTION MDD2000_DriveAxisDiag::SetDiagMode
	VAR_INPUT
		bStartDiagMode 	: BOOL;
		pThis 	: ^void;
	END_VAR
	VAR_OUTPUT
		retcode 	: DINT;
	END_VAR
  VAR
  	tmpPara : CmdStruct;
    tmpResult : results;
  END_VAR

  //Start Diag mode at axis
  tmpPara.uiCmd     := MDD2000MNG_IF_SET_DIAG_MODE;
  tmpPara.aPara[0]  := MDD2000MNG_IF_SET_DIAG_MODE_VER_1;
  tmpPara.aPara[1]  := TO_DINT(bStartDiagMode);
  tmpPara.aPara[2]  := pThis$DINT;
  DriveIF.NewInst(pPara:=#tmpPara, pResult:=#tmpResult);
  retcode := tmpResult.aData[4]$DINT; 

END_FUNCTION
