diff --git a/lcls-twincat-motion/Library/DUTs/DS402/ENUM_AxisStates.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/ENUM_AxisStates.TcDUT
deleted file mode 100644
index 669ab7e1..00000000
--- a/lcls-twincat-motion/Library/DUTs/DS402/ENUM_AxisStates.TcDUT
+++ /dev/null
@@ -1,16 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<TcPlcObject Version="1.1.0.1">
-  <DUT Name="ENUM_AxisStates" Id="{b411596a-bcb9-4bc6-9e2c-702c6c42053f}">
-    <Declaration><![CDATA[{attribute 'qualified_only'}
-{attribute 'to_string'}
-TYPE ENUM_AxisStates :
-(
-    UNKNOWN := 0, // UNKNOWN must be in slot 0 or the FB breaks
-    OUT := 1,
-    YAG := 2,
-    TT := 3
-) UINT;
-END_TYPE
-]]></Declaration>
-  </DUT>
-</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/DUTs/DS402/E_MoveDoneCause.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/E_MoveDoneCause.TcDUT
new file mode 100644
index 00000000..fbec8742
--- /dev/null
+++ b/lcls-twincat-motion/Library/DUTs/DS402/E_MoveDoneCause.TcDUT
@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="E_MoveDoneCause" Id="{f11d0b8d-a319-4919-8ad2-0dfa2b932888}">
+    <Declaration><![CDATA[{attribute 'qualified_only'}
+{attribute 'strict'}
+{attribute 'to_string'}
+TYPE E_MoveDoneCause :
+(
+    Unknown := 0,           // Default/uncategorized
+    MoveDone,               // Absolute move completed
+    StepMoveDone,           // Step/JOG move completed
+    HomeDone,               // Homing completed
+    CalibrationDone,        // Calibration completed
+    ResetDone,              // Reset command completed
+    HaltDone,               // Manual halt completed
+    AbortedHome,            // Home aborted
+    AbortedStepMove,        // Step move aborted
+    AbortedCalibration,     // Calibration aborted
+    Referenced,             // Axis became referenced
+    Calibrated              // Axis became calibrated
+);
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/DUTs/DS402/E_StartupState.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/E_StartupState.TcDUT
new file mode 100644
index 00000000..8705337e
--- /dev/null
+++ b/lcls-twincat-motion/Library/DUTs/DS402/E_StartupState.TcDUT
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="E_StartupState" Id="{a438f9d9-85e4-4a52-b694-851762491b8a}">
+    <Declaration><![CDATA[{attribute 'qualified_only'}
+{attribute 'strict'}
+{attribute 'to_string'}
+TYPE E_StartupState :
+(
+    STARTUP_IDLE:=0,
+    STARTUP_CLEAR,
+    STARTUP_RESET_FAULT,
+    STARTUP_SHUTDOWN,
+    STARTUP_SWITCH_ON,
+    STARTUP_ENABLE_OP
+);
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/DUTs/DS402/ST_ChannelState.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/ST_ChannelState.TcDUT
new file mode 100644
index 00000000..41a7eb4c
--- /dev/null
+++ b/lcls-twincat-motion/Library/DUTs/DS402/ST_ChannelState.TcDUT
@@ -0,0 +1,142 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="ST_ChannelState" Id="{57a8c6a5-d804-4041-a922-2988413bdf86}">
+    <Declaration><![CDATA[TYPE ST_ChannelState :
+STRUCT
+    {attribute 'pytmc' := '
+        pv: nChannelStateRaw
+        io: i
+        field: DESC Channel state raw
+    '}
+    nChannelStateRaw  : DWORD;
+
+    {attribute 'pytmc' := '
+        pv: bActivelyMoving
+        io: i
+        field: DESC Actively moving
+    '}
+    bActivelyMoving         : BOOL;
+    {attribute 'pytmc' := '
+        pv: bClosedLoopActive
+        io: i
+        field: DESC Closed loop active
+    '}
+    bClosedLoopActive       : BOOL;
+    {attribute 'pytmc' := '
+        pv: bCalibrating
+        io: i
+        field: DESC Calibrating
+    '}
+    bCalibrating            : BOOL;
+    {attribute 'pytmc' := '
+        pv: bReferencing
+        io: i
+        field: DESC Referencing
+    '}
+    bReferencing            : BOOL;
+    {attribute 'pytmc' := '
+        pv: bMoveDelayed
+        io: i
+        field: DESC Move delayed
+    '}
+    bMoveDelayed            : BOOL;
+    {attribute 'pytmc' := '
+        pv: bSensorPresent
+        io: i
+        field: DESC Sensor present
+    '}
+    bSensorPresent          : BOOL;
+    {attribute 'pytmc' := '
+        pv: bIsCalibrated
+        io: i
+        field: DESC Is calibrated
+    '}
+    bIsCalibrated           : BOOL;
+    {attribute 'pytmc' := '
+        pv: bIsReferenced
+        io: i
+        field: DESC Is referenced
+    '}
+    bIsReferenced           : BOOL;
+    {attribute 'pytmc' := '
+        pv: bEndStopReached
+        io: i
+        field: DESC End stop reached
+    '}
+    bEndStopReached         : BOOL;
+    {attribute 'pytmc' := '
+        pv: bRangeLimitReached
+        io: i
+        field: DESC Range limit reached
+    '}
+    bRangeLimitReached      : BOOL;
+    {attribute 'pytmc' := '
+        pv: bFollowingLimitReached
+        io: i
+        field: DESC Following limit reached
+    '}
+    bFollowingLimitReached  : BOOL;
+    {attribute 'pytmc' := '
+        pv: bMovementFailed
+        io: i
+        field: DESC Movement failed
+    '}
+    bMovementFailed         : BOOL;
+    {attribute 'pytmc' := '
+        pv: bIsStreaming
+        io: i
+        field: DESC Is streaming
+    '}
+    bIsStreaming            : BOOL;
+    {attribute 'pytmc' := '
+        pv: bPositionerOverload
+        io: i
+        field: DESC Positioner overload
+    '}
+    bPositionerOverload     : BOOL;
+    {attribute 'pytmc' := '
+        pv: bOverTemperature
+        io: i
+        field: DESC Over temperature
+    '}
+    bOverTemperature        : BOOL;
+    {attribute 'pytmc' := '
+        pv: bReferenceMark
+        io: i
+        field: DESC Reference mark
+    '}
+    bReferenceMark          : BOOL;
+    {attribute 'pytmc' := '
+        pv: bIsPhased
+        io: i
+        field: DESC Is phased
+    '}
+    bIsPhased               : BOOL;
+    {attribute 'pytmc' := '
+        pv: bPositionerFault
+        io: i
+        field: DESC Positioner fault
+    '}
+    bPositionerFault        : BOOL;
+    {attribute 'pytmc' := '
+        pv: bAmplifierEnabled
+        io: i
+        field: DESC Amplifier enabled
+    '}
+    bAmplifierEnabled       : BOOL;
+    {attribute 'pytmc' := '
+        pv: bInPosition
+        io: i
+        field: DESC In position
+    '}
+    bInPosition             : BOOL;
+    {attribute 'pytmc' := '
+        pv: bBrakeEnabled
+        io: i
+        field: DESC Brake enabled
+    '}
+    bBrakeEnabled           : BOOL;
+END_STRUCT
+END_TYPE]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/DUTs/DS402/ST_DS402Drive.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/ST_DS402Drive.TcDUT
index ddfe7c3c..8e479312 100644
--- a/lcls-twincat-motion/Library/DUTs/DS402/ST_DS402Drive.TcDUT
+++ b/lcls-twincat-motion/Library/DUTs/DS402/ST_DS402Drive.TcDUT
@@ -4,16 +4,16 @@
     <Declaration><![CDATA[TYPE ST_DS402Drive:
 STRUCT
     (* Global variables for the TPDO data from the drive. Each one is linked to the respective TPDO channel. *)
-    stDS402DriveStatus 		AT %I*	: 	ST_DS402DriveStatus;
+    stDriveStatus 			AT %I*	: 	ST_DS402DriveStatus;
     nModeOfOperationDisplay AT %I*	: 	SINT;
     //nActualPosition 		AT %I*	: 	DINT;
     (*CSP drive mode*)
     nFollowingError 		AT %I*	: 	DINT;
     (* Global variables for the RPDO data to the drive. Each one is linked to the respective RPDO channel. *)
     // connected to I/O Task 'Control Word'
-    nDS402DriveControl		AT %Q*	:	UINT;
+    nDriveControl			AT %Q*	:	UINT;
     // Connected to NC task Ctrl 1 and Ctrl 2
-    nDS402DriveControlNC	AT %I*	:	UINT;
+    nDriveControlNC			AT %I*	:	UINT;
     nTargetPosition 		AT %Q* 	: 	DINT;
     nModeOfOperation 		AT %Q* 	: 	SINT;
     nModeOfOperationNC 		AT %I* 	: 	SINT;
diff --git a/lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT b/lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT
new file mode 100644
index 00000000..beae5cb5
--- /dev/null
+++ b/lcls-twincat-motion/Library/DUTs/DS402/ST_MCS2ChanParameter.TcDUT
@@ -0,0 +1,213 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="ST_MCS2ChanParameter" Id="{ed228747-71e3-413e-a649-3dc97b8c806c}">
+    <Declaration><![CDATA[TYPE ST_MCS2ChanParameter :
+STRUCT
+    {attribute 'pytmc' := '
+        pv: nChanStep
+        io: io
+        field: DESC Open Loop Step Count
+    '}
+    nChanStep	: DINT := 500;
+
+    {attribute 'pytmc' := '
+        pv: nChanStepEgu
+        io: io
+        field: DESC Step scale factor
+    '}
+    nChanStepEgu	: LREAL := 827.15;
+
+    {attribute 'pytmc' := '
+        pv: nChanStepFreq
+        io: io
+        field: DESC Step Frequency
+    '}
+    nChanStepFreq	: UINT := 500;
+
+    {attribute 'pytmc' := '
+        pv: nChanStepAmp
+        io: io
+        field: DESC Step Amplitude
+    '}
+    nChanStepAmp	: UINT := 50;
+
+    {attribute 'pytmc' := '
+        pv: nHoldTime
+        io: io
+        field: DESC Close loop hold time
+    '}
+    nHoldTime	: UDINT := 30000;
+
+    {attribute 'pytmc' := '
+        pv: bStepMoveCmd
+        io: io
+        field: ZNAM DISABLE
+        field: ONAM ENABLE
+        field: DESC Used to disable close loop mode
+    '}
+    bStepMoveCmd	: BOOL;
+
+    {attribute 'pytmc' := '
+        pv: nMaxCloseLoopFreq
+        io: io
+        field: DESC Max Close Loop Freq
+    '}
+    nMaxCloseLoopFreq     : UDINT;
+
+    {attribute 'pytmc' := '
+        pv: nPositionerType
+        io: io
+        field: DESC Positioner Type
+    '}
+    nPositionerType	: UDINT;
+
+    {attribute 'pytmc' := '
+        pv: nCalibrationOpt
+        io: io
+        field: DESC Calibration Options
+    '}
+    nCalibrationOpt       : DWORD;
+
+    {attribute 'pytmc' := '
+        pv: nSensorPowerMode
+        io: io
+        field: DESC Sensor Power Mode
+    '}
+    nSensorPowerMode      : UDINT;
+
+    {attribute 'pytmc' := '
+        pv: nReferencingOpt
+        io: io
+        field: DESC Referencing Options
+    '}
+    nReferencingOpt       	   : DWORD;
+
+    {attribute 'pytmc' := '
+        pv: nReferenceType
+        io: i
+        field: DESC Reference Type
+    '}
+    nReferenceType 	: UDINT;
+
+    {attribute 'pytmc' := '
+        pv: nSafeDir
+        io: io
+        field: DESC Safe Direction
+    '}
+    nSafeDir	: BOOL;
+
+    {attribute 'pytmc' := '
+        pv: nMotorLoad
+        io: i
+        field: DESC Motor Load
+    '}
+    nMotorLoad	: USINT;
+
+    {attribute 'pytmc' := '
+        pv: nChannelType
+        io: io
+        field: DESC Channel Type
+    '}
+    nChannelType          : UDINT;
+
+    {attribute 'pytmc' := '
+        pv: nLogicalScaleOfs
+        io: io
+        field: DESC Logical Scale Offset
+    '}
+    nLogicalScaleOffset   : UDINT; // should LINT
+
+    {attribute 'pytmc' := '
+        pv: nLogicalScaleInv
+        io: io
+        field: DESC Logical Scale Inversion
+    '}
+    nLogicalScaleInv  	 : BOOL;
+
+    {attribute 'pytmc' := '
+        pv: bCalibrationCmd
+        io: io
+        field: DESC Logical Scale Offset
+    '}
+    bCalibrationCmd : BOOL;   // External request to switch to calibration mode
+
+    {attribute 'pytmc' := '
+        pv: chanState
+        io: i
+        field: DESC Channel configuration parameters.
+    '}
+    stChannelState         : ST_ChannelState;
+
+    {attribute 'hide'}
+    nHomeVeloFast         : UDINT;
+    {attribute 'hide'}
+    nSoftLimMin           : LINT;
+    {attribute 'hide'}
+    nSoftLimMax           : LINT;
+    {attribute 'hide'}
+    nHomeVeloSlow         : UDINT;
+    {attribute 'hide'}
+    nHomeAcc              : UDINT;
+    {attribute 'hide'}
+    nScaledFErrWin        : DINT;
+    {attribute 'hide'}
+    nHomeOffset           : DINT;
+    {attribute 'hide'}
+    fScalingFactor		  : LREAL := 0.000001;
+    {attribute 'hide'}
+    fHomeAcc			  : LREAL := 0.8;
+    // ====== Channel-specific object indices ======
+    nSubIndex                : BYTE := 0;        // Main subindex (which channel)
+    {attribute 'hide'}
+    nChanFErrWinIdx          : WORD;
+    {attribute 'hide'}
+    nChanMotorLoadIdx        : WORD;
+    {attribute 'hide'}
+    nChanErrorCodeIdx        : WORD;
+    {attribute 'hide'}
+    nChanStepIdx             : WORD;
+    {attribute 'hide'}
+    nChanStepFreqIdx         : WORD;
+    {attribute 'hide'}
+    nChanStepAmpIdx          : WORD;
+    {attribute 'hide'}
+    nChanSoftLimitIdx        : WORD;
+    {attribute 'hide'}
+    nChanHomeOffsIdx         : WORD;
+    {attribute 'hide'}
+    nChanHomeVeloIdx         : WORD;
+    {attribute 'hide'}
+    nChanhomeAccIdx          : WORD;
+    {attribute 'hide'}
+    nCHanMaxCloseLoopFreqIdx : WORD;
+    {attribute 'hide'}
+    nChanPositionerTypeIdx   : WORD;
+    {attribute 'hide'}
+    nChanCalibrationOptIdx   : WORD;
+    {attribute 'hide'}
+    nChanSensorPowerModeIdx  : WORD;
+    {attribute 'hide'}
+    nChanReferencingOptIdx   : WORD;
+    {attribute 'hide'}
+    nChanReferenceTypeIdx    : WORD;
+    {attribute 'hide'}
+    nChanSafeDirIdx          : WORD;
+    {attribute 'hide'}
+    nChannelTypeIdx          : WORD;
+    {attribute 'hide'}
+    nChanStateIdx            : WORD;
+    {attribute 'hide'}
+    nChanLogicalScaleOfsIdx  : WORD;
+    {attribute 'hide'}
+    nChanLogicalScaleInvIdx  : WORD;
+    fPosition : LREAL;
+    bStageReady : BOOL;
+    // Temporary workaround: Avoid declaring mc_power externally from FB_MotionStage
+    bOverrideDirEnable : BOOL := TRUE;
+    bEnableHardwareOps : BOOL := TRUE;
+    nReadTime : TIME := T#5S;
+END_STRUCT
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/DUTs/E_EpicsMotorCmd.TcDUT b/lcls-twincat-motion/Library/DUTs/E_EpicsMotorCmd.TcDUT
index 5054791b..48663950 100644
--- a/lcls-twincat-motion/Library/DUTs/E_EpicsMotorCmd.TcDUT
+++ b/lcls-twincat-motion/Library/DUTs/E_EpicsMotorCmd.TcDUT
@@ -12,9 +12,10 @@ TYPE E_EpicsMotorCmd :
     MOVE_ABSOLUTE := 3,
     MOVE_MODULO := 4,
     HOME := 10,
-    GEAR := 30
+    GEAR := 30,
+    CALIBRATE:=40
 );
 END_TYPE
 ]]></Declaration>
   </DUT>
-</TcPlcObject>
\ No newline at end of file
+</TcPlcObject>
diff --git a/lcls-twincat-motion/Library/Library.plcproj b/lcls-twincat-motion/Library/Library.plcproj
index 5418dbb1..9c54c057 100644
--- a/lcls-twincat-motion/Library/Library.plcproj
+++ b/lcls-twincat-motion/Library/Library.plcproj
@@ -18,7 +18,7 @@
     <Company>SLAC</Company>
     <Released>false</Released>
     <Title>lcls-twincat-motion</Title>
-    <ProjectVersion>0.0.0</ProjectVersion>
+    <ProjectVersion>1.0.0</ProjectVersion>
     <WriteProductVersion>false</WriteProductVersion>
     <POUsForPropertyAccessIncluded>false</POUsForPropertyAccessIncluded>
     <GlobalVersionStructureIncluded>false</GlobalVersionStructureIncluded>
@@ -32,6 +32,7 @@
     <Folder Include="DUTs\DS402" />
     <Folder Include="GVLs" />
     <Folder Include="POUs\Motion\DS402" />
+    <Folder Include="POUs\Motion\DS402\MCS2" />
     <Folder Include="POUs\Motion\PMPS" />
     <Folder Include="POUs\Motion\States\Examples" />
     <Folder Include="POUs\Motion\States\Helpers" />
@@ -109,9 +110,6 @@
     <Compile Include="DUTs\Deprecated\ENUM_StatePMPSStatus.TcDUT">
       <SubType>Code</SubType>
     </Compile>
-    <Compile Include="DUTs\DS402\ENUM_AxisStates.TcDUT">
-      <SubType>Code</SubType>
-    </Compile>
     <Compile Include="DUTs\DS402\E_DS402DriveState.TcDUT">
       <SubType>Code</SubType>
     </Compile>
@@ -130,18 +128,30 @@
     <Compile Include="DUTs\DS402\E_Module.TcDUT">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="DUTs\DS402\E_MoveDoneCause.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="DUTs\DS402\E_MoveMode.TcDUT">
       <SubType>Code</SubType>
     </Compile>
     <Compile Include="DUTs\DS402\E_MoveState.TcDUT">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="DUTs\DS402\E_StartupState.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="DUTs\DS402\ST_ChannelState.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="DUTs\DS402\ST_DS402Drive.TcDUT">
       <SubType>Code</SubType>
     </Compile>
     <Compile Include="DUTs\DS402\ST_DS402DriveStatus.TcDUT">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="DUTs\DS402\ST_MCS2ChanParameter.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="DUTs\E_EpicsHomeCmd.TcDUT">
       <SubType>Code</SubType>
     </Compile>
@@ -157,9 +167,6 @@
     <Compile Include="DUTs\E_StatePMPSStatus.TcDUT">
       <SubType>Code</SubType>
     </Compile>
-    <Compile Include="DUTs\ST_MotionEpicsInterface.TcDUT">
-      <SubType>Code</SubType>
-    </Compile>
     <Compile Include="DUTs\ST_MotionStage.TcDUT">
       <SubType>Code</SubType>
     </Compile>
@@ -271,10 +278,40 @@
     <Compile Include="POUs\Motion\DS402\FB_MotionStageE727.TcPOU">
       <SubType>Code</SubType>
     </Compile>
-    <Compile Include="POUs\Motion\DS402\FB_MotionStageMCS2.TcPOU">
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MCS2Calibrate.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MCS2DriveManager.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MCS2Home.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MCS2ReadParams.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MCS2StepMode.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_MotionStageMCS2.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_ReadChanParameters.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_ReadDriveCodes.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_WriteCalibrationParameters.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_WriteChanParameters.TcPOU">
+      <SubType>Code</SubType>
+    </Compile>
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_WriteHomeParameters.TcPOU">
       <SubType>Code</SubType>
     </Compile>
-    <Compile Include="POUs\Motion\DS402\I_DS402GenericDrive.TcIO">
+    <Compile Include="POUs\Motion\DS402\MCS2\FB_WriteStepModeParameters.TcPOU">
       <SubType>Code</SubType>
     </Compile>
     <Compile Include="POUs\Motion\FB_EncoderValue.TcPOU">
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageE727.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageE727.TcPOU
index 181146fd..65ba00db 100644
--- a/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageE727.TcPOU
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageE727.TcPOU
@@ -6,7 +6,7 @@
     set bServo PV to true, fort open loop step mode bServo must be cleared
 *)
 {attribute 'call_after_init'}
-FUNCTION_BLOCK FB_MotionStageE727 IMPLEMENTS I_DS402GenericDrive
+FUNCTION_BLOCK FB_MotionStageE727
 VAR
     {attribute 'no_copy'}
     stMotionStage  : REFERENCE TO ST_MotionStage;
@@ -173,7 +173,7 @@ IF bInit THEN
     stMotionStage.nMotionAxisID := stMotionStage.Axis.NcToPlc.AxisId;
     (* Clear drive error/wraning condition, in open loop mode
        this will trigger a correct startup for procedure *)
-    stDS402Drive.nDS402DriveControl := 128;
+    stDS402Drive.nDriveControl := 128;
     // fist cycle reading of NC paramters
     bExecParamsRead := TRUE;
     bInit := FALSE;
@@ -279,7 +279,7 @@ bExecMove:=bLocalExec AND NOT bExecHome;
 rtExec(CLK:=bLocalExec);
 IF rtExec.Q THEN
     // Release previous interruption if any
-    stDS402Drive.nDS402DriveControl.8 := FALSE;
+    stDS402Drive.nDriveControl.8 := FALSE;
     stMotionStage.bDone := FALSE;
 END_IF
 
@@ -307,7 +307,7 @@ IF stMotionStage.bReset THEN
     stMotionStage.sCustomErrorMessage:='';
     (*Manual Drive reset*)
     IF bDS402ManualEnabled THEN
-        stDS402Drive.nDS402DriveControl := 128;
+        stDS402Drive.nDriveControl := 128;
     ELSE
         bLocalReset := TRUE;
     END_IF
@@ -336,10 +336,10 @@ CASE nCommandLocal OF
         MoveAbsolute();
         (*IN CSP mode the drive control word is update via the NC
           Manual step mode and Homing will override this *)
-         stDS402Drive.nDS402DriveControl := stDS402Drive.nDS402DriveControlNC;
+         stDS402Drive.nDriveControl := stDS402Drive.nDriveControlNC;
     E_EpicsMotorCmd.HOME:
         (*DS402 Manual Homing*)
-       Home();
+       Home(Enable:=TRUE);
 END_CASE
 
 stMotionStage.bBusy := bHomeBusy OR fbMcMoveAbsolute.Busy;
@@ -514,7 +514,9 @@ THIS^.eModule := eModule;
     </Method>
     <Method Name="Home" Id="{33a0ebff-5827-46e7-8879-a94910cac0f9}">
       <Declaration><![CDATA[METHOD PUBLIC Home
-
+VAR_INPUT
+    Enable 				: BOOL;
+END_VAR
 VAR_INST
     {attribute 'hide'}
     bMove 				: BOOL;
@@ -539,16 +541,16 @@ IF bMove THEN
                 bHomeDone := FALSE;
                 bHomeBusy := TRUE;
                 stMotionStage.bDone := FALSE;
-                stDS402Drive.nDS402DriveControl:=15;
+                stDS402Drive.nDriveControl:=15;
                 eHomeState :=  E_MoveState.INIT;
             END_IF
         E_MoveState.INIT:
-                        stDS402Drive.nDS402DriveControl.4:=1;
+                        stDS402Drive.nDriveControl.4:=1;
                                         eHomeState := E_MoveState.STARTED;
         E_MoveState.STARTED :
             // this is a comfirmation that routine is ongoing
-            IF NOT stDS402Drive.stDS402DriveStatus.TargetReached
-                AND NOT stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific THEN
+            IF NOT stDS402Drive.stDriveStatus.TargetReached
+                AND NOT stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
                 bHomeBusy:=TRUE;
                 bHomeDone:=FALSE;
 
@@ -558,20 +560,20 @@ IF bMove THEN
         E_MoveState.IN_PROGRESS :
 
         IF bStop THEN
-         stDS402Drive.nDS402DriveControl.8 :=1;
+         stDS402Drive.nDriveControl.8 :=1;
           bHomeAborted := TRUE;
           eHomeState:=E_MoveState.INTERRUPTED;
 
         // Genral Motion error i.e Following error ?
-        ELSIF stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific
-                OR stDS402Drive.stDS402DriveStatus.WarningCondition THEN
+        ELSIF stDS402Drive.stDriveStatus.Bit13_OpModeSpecific
+                OR stDS402Drive.stDriveStatus.WarningCondition THEN
             stMotionStage.bError := TRUE;
             eHomeState:=E_MoveState.ERROR;
 
         // Reached
-        ELSIF stDS402Drive.stDS402DriveStatus.TargetReached
-            AND stDS402Drive.stDS402DriveStatus.Bit12_OpModeSpecific
-            AND NOT stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific THEN
+        ELSIF stDS402Drive.stDriveStatus.TargetReached
+            AND stDS402Drive.stDriveStatus.Bit12_OpModeSpecific
+            AND NOT stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
            eHomeState:=E_MoveState.DONE;
         END_IF
         E_MoveState.INTERRUPTED:
@@ -583,10 +585,10 @@ IF bMove THEN
             stMotionStage.fPosition:=0.0;
 
             IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-                stDS402Drive.nDS402DriveControl.4:=FALSE;
+                stDS402Drive.nDriveControl.4:=FALSE;
 
             ELSE
-                stDS402Drive.nDS402DriveControl:=6;
+                stDS402Drive.nDriveControl:=6;
             END_IF
             eHomeState:=E_MoveState.IDLING;
         E_MoveState.ERROR:
@@ -625,8 +627,8 @@ bNegativeDirection:=stMotionStage.bBusy AND stMotionStage.fPosDiff < 0;
 
 // For SmartAct MCS2 these limit are range and enstop limits. however a single bit in the status is used
 // bLimOverride  : act as an override after limit was hit to allow reverse movement
-stMotionStage.bLimitForwardEnable :=  bLimOverride OR NOT( bPositiveDirection AND stDS402Drive.stDS402DriveStatus.InternalLimitActive );
-stMotionStage.bLimitBackwardEnable := bLimOverride OR NOT( bNegativeDirection AND stDS402Drive.stDS402DriveStatus.InternalLimitActive );
+stMotionStage.bLimitForwardEnable :=  bLimOverride OR NOT( bPositiveDirection AND stDS402Drive.stDriveStatus.InternalLimitActive );
+stMotionStage.bLimitBackwardEnable := bLimOverride OR NOT( bNegativeDirection AND stDS402Drive.stDriveStatus.InternalLimitActive );
 
 // use falling trigger to avoid spaming  sErrorMessage
 ftForwardEnabled(CLK:=stMotionStage.bLimitForwardEnable);
@@ -675,13 +677,13 @@ IF rtCSPModeOk.Q THEN
 ELSIF rtHomeModeEnable.Q THEN
     bHomeModeEnabled := TRUE;
     bCSPModeEnabled := FALSE;
-    stDS402Drive.nDS402DriveControl := 0;
+    stDS402Drive.nDriveControl := 0;
 END_IF
 
 IF rtUserExec.Q AND stMotionStage.bHomeCmd THEN
     IF (stDS402Drive.nModeOfOperationDisplay<>E_DS402OpMode.HOME) THEN
         stDS402Drive.nModeOfOperation := E_DS402OpMode.HOME;
-        stDS402Drive.nDS402DriveControl := 128;
+        stDS402Drive.nDriveControl := 128;
         // clear previous operational from switching away from this mode.
         bOperational := FALSE;
         bCSPModeEnabled := FALSE;
@@ -692,7 +694,7 @@ IF rtUserExec.Q AND stMotionStage.bHomeCmd THEN
 ELSIF rtUserExec.Q THEN
     IF (stDS402Drive.nModeOfOperationDisplay<>E_DS402OpMode.CSP) THEN
         stDS402Drive.nModeOfOperation := E_DS402OpMode.CSP;
-        stDS402Drive.nDS402DriveControl := 128;
+        stDS402Drive.nDriveControl := 128;
         bHomeModeEnabled := FALSE;
         bManualTransition := FALSE;
     ELSE
@@ -942,24 +944,6 @@ stMotionStage.fPosDiff:=stMotionStage.Axis.NcToPlc.PosDiff;
 ]]></ST>
       </Implementation>
     </Method>
-    <Method Name="ScaleHomeMotionParams" Id="{ca311234-dfe1-4f79-8d46-d85b94fbe633}">
-      <Declaration><![CDATA[(*Scale Home Move parameters*)
-METHOD ScaleHomeMotionParams
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ScaleServoOffMotionParams" Id="{c3995288-2bc6-4e2b-80eb-94f29f532c0d}">
-      <Declaration><![CDATA[(*Scaled Open Loop Step mode motion parameters*)
-METHOD ScaleServoOffMotionParams
-VAR_INPUT
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[]]></ST>
-      </Implementation>
-    </Method>
     <Method Name="SetEnables" Id="{40234a90-944e-46f2-a31c-ab09920999d8}">
       <Declaration><![CDATA[METHOD SetEnables
 VAR_INPUT
@@ -987,59 +971,59 @@ END_VAR]]></Declaration>
       <Implementation>
         <ST><![CDATA[rtFault(CLK:=bFault);
 (*switch on disable*)
-IF NOT stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND  stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
+IF NOT stDS402Drive.stDriveStatus.ReadyToSwitchOn
+    AND NOT stDS402Drive.stDriveStatus.SwitchedOn
+    AND NOT stDS402Drive.stDriveStatus.OperationEnabled
+    AND NOT stDS402Drive.stDriveStatus.Fault
+    AND stDS402Drive.stDriveStatus.VoltageEnabled
+    AND NOT stDS402Drive.stDriveStatus.QuickStopActive
+    AND  stDS402Drive.stDriveStatus.SwitchOnDisabled THEN
         bOperational := FALSE;
         IF bDS402ManualEnabled AND NOT bStop  THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 6;
+            THIS^.stDS402Drive.nDriveControl := 6;
         END_IF
 (*Ready to switch*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
+ELSIF stDS402Drive.stDriveStatus.ReadyToSwitchOn
+    AND NOT stDS402Drive.stDriveStatus.SwitchedOn
+    AND NOT stDS402Drive.stDriveStatus.OperationEnabled
+    AND NOT stDS402Drive.stDriveStatus.Fault
+    AND stDS402Drive.stDriveStatus.VoltageEnabled
+    AND stDS402Drive.stDriveStatus.QuickStopActive
+    AND NOT stDS402Drive.stDriveStatus.SwitchOnDisabled THEN
         bFault := FALSE;
         bOperational := FALSE;
         IF bDS402ManualEnabled AND NOT bStop  THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 7;
+            THIS^.stDS402Drive.nDriveControl := 7;
         END_IF
 (*Switch on*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND  stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
+ELSIF stDS402Drive.stDriveStatus.ReadyToSwitchOn
+    AND  stDS402Drive.stDriveStatus.SwitchedOn
+    AND NOT stDS402Drive.stDriveStatus.OperationEnabled
+    AND NOT stDS402Drive.stDriveStatus.Fault
+    AND stDS402Drive.stDriveStatus.VoltageEnabled
+    AND stDS402Drive.stDriveStatus.QuickStopActive
+    AND NOT stDS402Drive.stDriveStatus.SwitchOnDisabled THEN
         bOperational := FALSE;
         IF bDS402ManualEnabled AND NOT bStop  THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 15;
+            THIS^.stDS402Drive.nDriveControl := 15;
         END_IF
 (*Operation enbaled*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
+ELSIF stDS402Drive.stDriveStatus.ReadyToSwitchOn
+    AND stDS402Drive.stDriveStatus.SwitchedOn
+    AND stDS402Drive.stDriveStatus.OperationEnabled
+    AND NOT stDS402Drive.stDriveStatus.Fault
+    AND stDS402Drive.stDriveStatus.VoltageEnabled
+    AND stDS402Drive.stDriveStatus.QuickStopActive
+    AND NOT stDS402Drive.stDriveStatus.SwitchOnDisabled THEN
         THIS^.bOperational := TRUE;
 (*Fault*)
-ELSIF  NOT stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
+ELSIF  NOT stDS402Drive.stDriveStatus.ReadyToSwitchOn
+    AND NOT stDS402Drive.stDriveStatus.SwitchedOn
+    AND NOT stDS402Drive.stDriveStatus.OperationEnabled
+    AND stDS402Drive.stDriveStatus.Fault
+    AND stDS402Drive.stDriveStatus.VoltageEnabled
+    AND NOT stDS402Drive.stDriveStatus.QuickStopActive
+    AND NOT stDS402Drive.stDriveStatus.SwitchOnDisabled THEN
         bOperational := FALSE;
         bFault := TRUE;
 
@@ -1053,17 +1037,6 @@ stMotionStage.bError := bFault AND stMotionStage.bAxisParamsInit;
 ]]></ST>
       </Implementation>
     </Method>
-    <Method Name="StepMove" Id="{beb62377-55f7-4812-963a-214f50a0e0dd}">
-      <Declaration><![CDATA[{warning 'Add method implementation '}
-METHOD StepMove
-VAR_INPUT
-    Enable	: BOOL;
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[]]></ST>
-      </Implementation>
-    </Method>
     <Method Name="UpdateDriveMonitoringParams" Id="{4485f39e-5aab-45c4-94ad-cec5d5ba73c4}">
       <Declaration><![CDATA[(*	Drive parameters update
     Mirror NC axis parameters configurations to those of MCS2 drive
@@ -1212,22 +1185,6 @@ stMotionStage.stAxisStatus.nCommand:=3; // If this is not 3, the IOC stops updat
 ]]></ST>
       </Implementation>
     </Method>
-    <Method Name="UpdateServoOffMotionParams" Id="{1eeb0577-3c90-46e7-82fa-847c998f0d69}">
-      <Declaration><![CDATA[(*	MCS2 OL Step parameters update
-    1- Step
-    2- Step Amplitude
-    3- StepFreq
-*)
-METHOD UpdateServoOffMotionParams
-VAR_INPUT
-    Enable : BOOL;
-END_VAR
-
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[]]></ST>
-      </Implementation>
-    </Method>
     <Method Name="WriteParameterNC" Id="{820518b9-7c36-46bd-907c-c13e63c9c459}">
       <Declaration><![CDATA[METHOD WriteParameterNC
 VAR_INPUT
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageMCS2.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageMCS2.TcPOU
deleted file mode 100644
index 4f6124a3..00000000
--- a/lcls-twincat-motion/Library/POUs/Motion/DS402/FB_MotionStageMCS2.TcPOU
+++ /dev/null
@@ -1,1739 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<TcPlcObject Version="1.1.0.1">
-  <POU Name="FB_MotionStageMCS2" Id="{8a1beccb-8ae6-4cef-98b6-1d3261aefb9e}" SpecialFunc="None">
-    <Declaration><![CDATA[(*	MCS2 Motion Stage . Controller a stage in CSP (NC), STEP_MODE and Homing via DS402 interface
-     Using an overriden Position Holding timer. power enable mode must be 'DURING_MOTION'. for close loop aka CSP control
-    set bServo PV to true, fort open loop step mode bServo must be cleared
-*)
-{attribute 'call_after_init'}
-FUNCTION_BLOCK FB_MotionStageMCS2 IMPLEMENTS I_DS402GenericDrive
-VAR
-    {attribute 'no_copy'}
-    stMotionStage  : REFERENCE TO ST_MotionStage;
-    // Hardware I/O
-    stDS402Drive 		: ST_DS402Drive;
-    // Must be Provided
-    {attribute 'hide'}
-    eModule 			: E_Module;
-    {attribute 'hide'}
-    eHomeMode			: E_EpicsHomeCmd :=E_EpicsHomeCmd.AUTOZERO;
-    {attribute 'hide'}
-    eBufferMode 		: MC_BufferMode := MC_BufferMode.MC_Aborting;
-    {attribute 'hide'}
-    eMotionState 		: E_MoveState;
-    {attribute 'hide'}
-    eHomeState  		: E_MoveState;
-    {attribute 'hide'}
-    fbMcPower 			: MC_Power;
-    {attribute 'hide'}
-    fbMcMoveAbsolute 	: MC_MoveAbsolute;
-    {attribute 'hide'}
-    fbMcHalt 			: MC_Halt;
-    {attribute 'hide'}
-    fbMcReset 			: MC_Reset;
-    {attribute 'hide'}
-    fbMcWriteParameter 	: MC_WriteParameter;
-    {attribute 'hide'}
-    fbMcReadParams		: MC_ReadParameterSet;
-    // MCS2 OL PVs
-    {attribute 'pytmc' := '
-        pv: PLC:nChanStep
-        io: io
-        field: DESC SmartAct: Open Loop Step Count
-    '}
-    nChanStep : DINT := 500;
-
-    {attribute 'pytmc' := '
-        pv: PLC:nChanStepEgu
-        io: io
-        field: DESC SmartAct: Open Loop Step scale factor
-    '}
-    nChanStepEgu: LREAL := 827.15;
-
-    {attribute 'pytmc' := '
-        pv: PLC:nChanStepFreq
-        io: io
-        field: DESC SmartAct: Open Loop Step Freq
-    '}
-    nChanStepFreq : UINT := 500;
-
-    {attribute 'pytmc' := '
-        pv: PLC:nChanStepAmp
-        io: io
-        field: DESC SmartAct: Open Loop Step Amp
-    '}
-    nChanStepAmp  : UINT := 50;
-    // timeout for holding actual position ~ 30s
-    {attribute 'pytmc' := '
-        pv: PLC:nHoldTime
-        io: io
-        field: DESC SmartAct: Set servo mode hold time
-    '}
-    nHoldTime : UDINT := 30000;
-    // Switch between closed and open loop operation
-    {attribute 'pytmc' := '
-        pv: PLC:bServo
-        io: io
-        field: ZNAM DISABLE
-        field: ONAM ENABLE
-        field: DESC Used to disable close loop mode
-    '}
-    bServo  			: BOOL := TRUE;
-   {attribute 'hide'}
-    bLastHoltTimeOut : BOOL;
-   {attribute 'hide'}
-    bEnPosLag 			: BOOL;
-    {attribute 'hide'}
-    bWrongParameter 	: BOOL;
-    {attribute 'hide'}
-    bExecMove 			: BOOL;
-    {attribute 'hide'}
-    bExecHome 			: BOOL;
-    {attribute 'hide'}
-    bNewMoveReq 		: BOOL;
-    {attribute 'hide'}
-    bHomeBusy 			: BOOL;
-    {attribute 'hide'}
-    bPrepareDisable 	: BOOL;
-    {attribute 'hide'}
-    bStepMoveDone 		: BOOL;
-    {attribute 'hide'}
-    bStepMoveBusy 		: BOOL;
-    {attribute 'hide'}
-    bStop  				: BOOL;
-    {attribute 'hide'}
-    bOperational  		: BOOL;
-    {attribute 'hide'}
-    bLimOverride  		: BOOL;
-    {attribute 'hide'}
-    bPositionHold  		: BOOL;
-    // use this for motion time axis states
-    {attribute 'hide'}
-    bLocalExec			: BOOL;
-    // Save abd restore encoder value
-    {attribute 'hide'}
-    bSystemInit				: BOOL;
-    bHomeDone 			: BOOL;
-    {attribute 'hide'}
-    bStepModeEnabled	: BOOL;
-    {attribute 'hide'}
-    bHomeModeEnabled	: BOOL;
-    {attribute 'hide'}
-    bCSPModeEnabled		: BOOL;
-    {attribute 'hide'}
-    bSteModeEnable 		: BOOL;
-    {attribute 'hide'}
-    bRestoreLoad		: BOOL;
-   {attribute 'hide'}
-    bStepModeparamsSetDone	: BOOL;
-    {attribute 'hide'}
-    bExecParamsRead			: BOOL;
-    {attribute 'hide'}
-    bCommandMoveAbsolute 	: BOOL;
-    {attribute 'hide'}
-    bRecentEnPosLagStatus 	: BOOL;
-    {attribute 'hide'}
-    bDS402ManualAborted : BOOL;
-    {attribute 'hide'}
-    bStepMoveAborted : BOOL;
-    {attribute 'hide'}
-    bHomeAborted : BOOL;
-    {attribute 'hide'}
-    bDS402ManualEnabled : BOOL;
-    {attribute 'hide'}
-    fScalededSteps  	: LREAL;
-    {attribute 'hide'}
-    fMeasuredVelo		: LREAL;
-    {attribute 'hide'}
-    fMeasuredAcc		: LREAL;
-    {attribute 'hide'}
-    fScalingFactor		: LREAL:= 0.000001;
-   {attribute 'hide'}
-    fHomeAcc  			: LREAL := 0.8;
-    fParameterValue : LREAL;
-    {attribute 'hide'}
-    fMeasuredPos 		: LREAL;
-    {attribute 'hide'}
-    nScalededStepAmp  	: UINT;
-    {attribute 'hide'}
-    nScaledStepFreq   	: UINT;
-    {attribute 'hide'}
-    nHomeVeloFast 		: UDINT;
-    {attribute 'hide'}
-    nSoftLimMin     	: LINT;
-    {attribute 'hide'}
-    nSoftLimMax     	: LINT;
-    {attribute 'hide'}
-    nHomeVeloSlow  		: UDINT;
-    {attribute 'hide'}
-    nHomeAcc 			: UDINT;
-    {attribute 'hide'}
-    {attribute 'hide'}
-    nScaledFErrWin 		: DINT;
-    {attribute 'hide'}
-    nHomeOffset 		: DINT;
-    {attribute 'hide'}
-    nScalededSteps  	: DINT;
-    {attribute 'hide'}
-    tonSyncHoming 		: TON;
-    {attribute 'hide'}
-    tonHoldTime     	: TON;
-    {attribute 'hide'}
-    tonSlowParamReadTimer: TON;
-    {attribute 'hide'}
-    NCParamsTimer 		: TON;
-    {attribute 'hide'}
-    tonRestoreRetry		: TON;
-    {attribute 'hide'}
-    rtAborted 			: R_TRIG;
-    {attribute 'hide'}
-    rtStepMoveDone 		: R_TRIG;
-    {attribute 'hide'}
-    rtStopDone			: R_TRIG;
-    {attribute 'hide'}
-    rtExec 				: R_TRIG;
-    {attribute 'hide'}
-    rtMoveCmdShortcut 	: R_TRIG;
-    {attribute 'hide'}
-    rtHomeCmdShortcut 	: R_TRIG;
-    {attribute 'hide'}
-    rtEnableMode    	: R_TRIG;
-    {attribute 'hide'}
-    rtUserExec      	: R_TRIG;
-    {attribute 'hide'}
-    rtModeChange      	: R_TRIG;
-    {attribute 'hide'}
-    rtMoveDone    		: R_TRIG;
-    {attribute 'hide'}
-    {attribute 'hide'}
-    rtNewMoveReq  		: R_TRIG;
-    {attribute 'hide'}
-    ftExec      		: F_TRIG;
-
-    {attribute 'hide'}
-    rtHomeDone 			: R_TRIG;
-    {attribute 'hide'}
-    rtCSPModeOk			: R_TRIG;
-    {attribute 'hide'}
-    rtHomeModeEnable	: R_TRIG;
-    {attribute 'hide'}
-    rtStepModeEnabled	: R_TRIG;
-    {attribute 'hide'}
-    rtNewServoMove		: R_TRIG;
-    {attribute 'hide'}
-    rtStepMove			: R_TRIG;
-    {attribute 'hide'}
-    nSubIndex			: BYTE := 0;
-    {attribute 'hide'}
-    nChanFErrWinIdx		: WORD;
-    {attribute 'hide'}
-    nChanSoftwareLimIdx : WORD;
-    {attribute 'hide'}
-    nChanMotorLoadIdx 	: WORD;
-    {attribute 'hide'}
-    nChanErrorCodeIdx 	: WORD;
-    {attribute 'hide'}
-    nChanStepIdx  		: WORD;
-    {attribute 'hide'}
-    nChanStepFreqIdx	: WORD;
-    {attribute 'hide'}
-    nChanStepAmpIdx 	: WORD;
-    {attribute 'hide'}
-    nChanSoftLimitIdx 	: WORD;
-    {attribute 'hide'}
-    nChanHomeOffsIdx 	: WORD;
-    {attribute 'hide'}
-    nChanHomeVeloIdx 	: WORD;
-    {attribute 'hide'}
-    nChanhomeAccIdx		: WORD;
-    {attribute 'hide'}
-    bLocalReset: BOOL;
-    {attribute 'hide'}
-    nCommandLocal: INT := 3;
-    {attribute 'hide'}
-    nCmdDataLocal: INT;
-   {attribute 'hide'}
-    bManualTransition: BOOL;
-   {attribute 'hide'}
-    tonInitTimer : TON;
-END_VAR
-
-VAR CONSTANT
-    {attribute 'hide'}
-    nChanSoftLimitBase	: WORD := 16#200E;
-    {attribute 'hide'}
-    nHomeOffsetBase 	: WORD := 16#607C;
-    {attribute 'hide'}
-    nHomingSpeedBase	: WORD := 16#6099;
-    {attribute 'hide'}
-    nhomingAccBase  	: WORD := 16#609A;
-    {attribute 'hide'}
-    nErrorCodeBase  	: WORD := 16#603F;
-    {attribute 'hide'}
-    nFErrWinBase  		: WORD := 16#6065;
-    // this true for SmartAct MCS2 and E-727.
-    {attribute 'hide'}
-    nChanOfsBase		:  WORD := 16#800;
-    // DS 402 Motor encoder defective code : this is needed for save and restore.
-    {attribute 'hide'}
-    nEncErrorCode 		: WORD := 16#7300;
-    {attribute 'hide'}
-    nSoftLimBase		: WORD := 16#200E;
-    {attribute 'hide'}
-    nOlStepBase 		: WORD := 16#2022;
-    {attribute 'hide'}
-    nOlStepAmpBase		: WORD := 16#2023;
-    {attribute 'hide'}
-    nOlStepFreqBase 	: WORD := 16#2024;
-END_VAR
-
-VAR PERSISTENT
-    bSaved	: BOOL;
-    fSavedPosition	: LREAL;
-END_VAR
-]]></Declaration>
-    <Implementation>
-      <ST><![CDATA[IF NOT __ISVALIDREF(stMotionStage) THEN
-    RETURN;
-END_IF
-
-// when the plc autoboot it appears reading from i/o is not yet after 'call_after_init'
-IF bSystemInit THEN
-    IF stMotionStage.Axis.NcToPlc.AxisId <> 0 THEN
-        stDS402Drive.nDS402DriveControl := 128; // clear errors, prepare for startup
-        bExecParamsRead := TRUE; // trigger NC param read on first scan
-        bSystemInit := FALSE;
-    END_IF
-END_IF
-(* Check for the plc shortcut commands
-   Used for testing or to circumvent motor record issues*)
-rtMoveCmdShortcut(CLK:=stMotionStage.bMoveCmd);
-rtHomeCmdShortcut(CLK:=stMotionStage.bHomeCmd);
-
-(* Execute on rising edge*)
-IF rtMoveCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
-    stMotionStage.bExecute:=TRUE;
-    stMotionStage.nCommand := E_EpicsMotorCmd.MOVE_ABSOLUTE;
-    bEnPosLag := bRecentEnPosLagStatus;
-    fParameterValue := BOOL_TO_LREAL(bRecentEnPosLagStatus);
-
-ELSIF rtHomeCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
-    stMotionStage.bExecute:= NOT ( stMotionStage.nCmdData=E_EpicsHomeCmd.ABSOLUTE_SET )
-                            OR NOT ( stMotionStage.nCmdData=E_EpicsHomeCmd.NONE);
-    stMotionStage.nCommand:=E_EpicsMotorCmd.HOME;
-    (* Automatically fill the correct nCmdData for homing*)
-    stMotionStage.nCmdData:=stMotionStage.nHomingMode;
-    (*	Positional lag is not evaluated by E727 during homing, but NC does evaluated
-        the condition which will lead to error as the lag is significant during homing
-        disable the NC lag check and reenable it after homing is done
-    *)
-    IF stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl.0 THEN
-        bRecentEnPosLagStatus := stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl.0;
-        bEnPosLag := TRUE;
-        fParameterValue := 0;
-    END_IF
-
-END_IF
-
-(* entry point for local and EPICS main execs *)
-rtUserExec(CLK:=stMotionStage.bExecute);
-(* if a Move/Home Goal is comfirmed and there is no persistant
-   error conditions, then we have a valid move request.*)
-bNewMoveReq S= NOT stMotionStage.bBusy AND rtUserExec.Q AND NOT stMotionStage.bError;
-(* this Move request is valid till an error occurs or the currently move is done *)
-bNewMoveReq R= NOT stMotionStage.bExecute OR stMotionStage.bError;
-bPrepareDisable R= bNewMoveReq;
-rtNewMoveReq(CLK:=bNewMoveReq);
-(* Moves are automatically allowed if no safety hooks.
-   Otherwise, some other code will set this.*)
-stMotionStage.bSafetyReady S= stMotionStage.bPowerSelf;
-(* Set the proper command for the request move;
-   if bservo not set, manual step moves will be performed *)
-IF rtUserExec.Q THEN
-    // Transfer nCommand and nCmdData to local copies at rising edge of bExecute (avoid issues if nCommand or nCmdData are changed during a command)
-    nCmdDataLocal:=stMotionStage.nCmdData;
-    nCommandLocal:=stMotionStage.nCommand;
-    IF NOT stMotionStage.bHomeCmd THEN
-        IF NOT bServo THEN
-            (* Open Loop Step Mode *)
-            stMotionStage.nCommand:=E_EpicsMotorCmd.JOG;
-            nCommandLocal := E_EpicsMotorCmd.JOG;
-        END_IF
-
-        (* attempting to move an axis without homing first? *)
-        IF stMotionStage.nHomingMode <> E_EpicsHomeCmd.NONE AND NOT stMotionStage.bHomed THEN
-            (* one can just set bHome here even though no homing was done?*)
-            stMotionStage.sErrorMessage:='Axis homing mode set, but homing routine pending';
-        END_IF
-    END_IF
-END_IF
-
-(* Set the drive in the correct operating mode. based on requested move command!
-    NB: bservo must set for close loop motion
-*)
-ModeOperation();
-
-(* NB: This is the only tested mode of operation so far. also aligned
-    piezo drive position holding feature
-*)
-rtEnableMode(CLK:=(stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION));
-(* Handle auto-enable timing *)
-CASE stMotionStage.nEnableMode OF
-    (* Not recommended, not tested *)
-    E_StageEnableMode.ALWAYS:
-        stMotionStage.bEnable:=TRUE;
-    E_StageEnableMode.DURING_MOTION:
-        IF rtEnableMode.Q THEN
-            stMotionStage.bEnable := FALSE;
-        END_IF
-        IF rtNewMoveReq.Q THEN
-            (* override ongoing holding.*)
-            bPositionHold := FALSE;
-            tonHoldTime.IN:=FALSE;
-            (*MC block are not compatible with open loop Motion
-                thus we wathc for correct close loop modes to use NC features *)
-            stMotionStage.bEnable S= stMotionStage.bSafetyReady;
-            bLastHoltTimeOut := FALSE;
-            bStop:=FALSE;
-        END_IF
-END_CASE
-(*	Interlock mainly react on internal limit conditions inthe ove direction.
-    MCS2 drive will go into error state when an internal limit is hit *)
-Interlock();
-
-IF bCSPModeEnabled THEN
-    (*CSP using TwinCAT NC*)
-    bLocalExec:= NOT stMotionStage.bError AND stMotionStage.bExecute AND stMotionStage.bAllEnable AND stMotionStage.bEnableDone AND stMotionStage.bSafetyReady AND bServo;
-ELSIF bHomeModeEnabled THEN
-    bLocalExec:= NOT stMotionStage.bError AND stMotionStage.bExecute AND stMotionStage.bAllEnable AND  bOperational AND  stMotionStage.bSafetyReady;
-ELSIF bStepModeEnabled THEN
-    (*Manual, i.e Homing using DS402*)
-    bLocalExec:= NOT stMotionStage.bError AND stMotionStage.bExecute AND stMotionStage.bAllEnable AND  bOperational AND  stMotionStage.bSafetyReady AND NOT bServo;
-ELSE
-    bLocalExec:= FALSE;
-END_IF
-bExecHome:=bLocalExec AND stMotionStage.nCommand = 10;
-bExecMove:=bLocalExec AND NOT bExecHome;
-
-(* When we start, set the busy/done appropriately
-   NB: CLose loop control using NC *)
-rtExec(CLK:=bLocalExec);
-IF rtExec.Q THEN
-    // Release previous interruption if any
-    stDS402Drive.nDS402DriveControl.8 := FALSE;
-    stMotionStage.bDone := FALSE;
-END_IF
-
-(* updated axis status
-   Not needed in manual mode *)
-IF NOT bDS402ManualEnabled THEN
-    stMotionStage.Axis.ReadStatus();
-END_IF
-
-(* Get a definitive bEnabled reading
-   NB: use only in close loop mode, NC feature needed*)
-CASE stMotionStage.Axis.Status.MotionState OF
-    (* We are not enabled if there is an issue*)
-    MC_AXISSTATE_UNDEFINED, MC_AXISSTATE_DISABLED, MC_AXISSTATE_ERRORSTOP:
-        stMotionStage.bEnableDone := FALSE;
-    ELSE
-        stMotionStage.bEnableDone := TRUE;
-END_CASE
-//
-IF stMotionStage.bReset THEN
-    stMotionStage.bExecute:=FALSE;
-    stMotionStage.bError := FALSE;
-    stMotionStage.nErrorId := 0;
-    stMotionStage.sErrorMessage:='';
-    stMotionStage.sCustomErrorMessage:='';
-    (*Manual Drive reset*)
-    IF bDS402ManualEnabled THEN
-        // Clear error/warning
-        bManualTransition:=FALSE;
-        IF NOT stMotionStage.bBusy THEN
-            stDS402Drive.nDS402DriveControl := 128;
-        ELSE
-            // Power off the drive
-            stMotionStage.bEnable:=FALSE;
-            stDS402Drive.nDS402DriveControl := 6;
-        END_IF
-    ELSE
-        bLocalReset := TRUE;
-    END_IF
-    stMotionStage.bReset:=FALSE;
-END_IF
-
-// Halt is always a user stop during motion
-bStop S= NOT bLocalExec AND NOT bLocalReset AND ((bHomeBusy AND (nCommandLocal=10))
-                        OR(fbMcMoveAbsolute.Busy AND (nCommandLocal=3)) OR (bStepMoveBusy AND (nCommandLocal=0)));
-
-//auto handle position lag monitoring, disable during homing.
-WriteParameterNC(	Execute:=bEnPosLag,
-                    ParameterNumber:=MC_AxisParameter.AxisEnPositionLagMonitoring,
-                    ParameterValue:=fParameterValue);
-
-(* Requested commands processing *)
-CASE nCommandLocal OF
-    E_EpicsMotorCmd.MOVE_ABSOLUTE:
-        (*NC CSP Move Handling*)
-        Reset();
-        Power();
-        Halt();
-        (*Wait for drive to be in the correct mode after a request was validated *)
-        bCommandMoveAbsolute := bExecMove AND fbMcPower.Status AND bCSPModeEnabled;
-        MoveAbsolute();
-        (*IN CSP mode the drive control word is update via the NC
-          Manual step mode and Homing will override this *)
-         stDS402Drive.nDS402DriveControl := stDS402Drive.nDS402DriveControlNC;
-    E_EpicsMotorCmd.HOME:
-        (*DS402 Manual Homing*)
-        Home();
-    E_EpicsMotorCmd.JOG:
-        (* Set drive to correct operating mode, ensure motion params are correct
-          following a validated open loop mode request.
-          NB: bservo must be off. a PV is provided for that
-        *)
-         StepMove(Enable:=bExecMove AND bStepModeparamsSetDone AND NOT bWrongParameter);
-END_CASE
-
-stMotionStage.bBusy := bHomeBusy OR fbMcMoveAbsolute.Busy OR bStepMoveBusy;
-
-(* Check done moving via user stop, limit hit, Target Position reached, or from homing.*)
-rtMoveDone(CLK:=fbMcMoveAbsolute.Done);
-rtStepMoveDone(CLK:=bStepMoveDone);
-rtHomeDone(CLK:=bHomeDone);
-rtStopDone(CLK:=fbMcHalt.Done);
-rtAborted(CLK:=bHomeAborted OR bStepMoveAborted);
-IF rtAborted.Q OR rtStopDone.Q OR rtMoveDone.Q OR rtHomeDone.Q OR rtStepMoveDone.Q THEN
-    IF NOT stMotionStage.bDone THEN
-        stMotionStage.bHomed := bHomeDone;
-        stMotionStage.bExecute:=FALSE;
-        bCommandMoveAbsolute := FALSE;
-        stMotionStage.bDone := fbMcMoveAbsolute.Done OR bHomeDone OR bStepMoveDone;
-        IF bDS402ManualEnabled THEN
-            (*Restore Postional lag monitoring*)
-            bEnPosLag := FALSE;
-        END_IF
-        (* Release the internal limit override*)
-        bLimOverride := FALSE;
-        bHomeAborted := FALSE;
-        bStepMoveAborted := FALSE;
-    END_IF
-END_IF
-
-(* hold stage in place before timeout *)
-tonHoldTime(PT:=UDINT_TO_TIME(nHoldTime));
-IF tonHoldTime.Q THEN
-    bPositionHold := FALSE;
-    tonHoldTime.IN:=FALSE;
-    bLastHoltTimeOut := TRUE;
-    (* open loop mode is not compatible with MC_Power, MC _Halt...*)
-    (* Manual power disabling is required*)
-    IF bDS402ManualEnabled THEN
-        stDS402Drive.nDS402DriveControl := 6;
-    END_IF
-    stMotionStage.bEnable:=FALSE;
-    bStop := FALSE;
-END_IF
-
-(* updated axis status
-   Not needed in manual mode *)
-IF NOT bDS402ManualEnabled THEN
-    stMotionStage.Axis.ReadStatus();
-END_IF
-//
-ftExec(CLK:=stMotionStage.bExecute);
-(*Handle auto-disable timing*)
-bPrepareDisable S= stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION AND ftExec.Q;
-(* Delay the disable until we reach standstill *)
-IF bPrepareDisable AND stMotionStage.Axis.Status.StandStill THEN
-    IF NOT bPositionHold THEN
-        bPrepareDisable:=FALSE;
-        stMotionStage.bEnable:=FALSE;
-    END_IF
-END_IF
-
-(* update encoder value and calibrated position*)
-ScaleEncRawValue();
-(*Drive parameters*)
-ExposedParameters(Enable:=TRUE, tRefreshDelay:=T#2S);
-(* Open Loop Motion paramters Update*)
-UpdateServoOffMotionParams(Enable:=stMotionStage.bAxisParamsInit AND NOT stMotionStage.bBusy AND bStepModeEnabled);
-(*Sync NC setting to drive settings*)
-UpdateDriveMonitoringParams(Enable:=stMotionStage.bAxisParamsInit AND NOT stMotionStage.bBusy);
-(* Save and restore as long as not an absolute encoder*)
-PersistParameters( Enable:=stMotionStage.nHomingMode <> E_EpicsHomeCmd.NONE);
-(*Restore encoder value at initialization*)
-RestoreMotionParams(Enable:=stMotionStage.nHomingMode <> E_EpicsHomeCmd.NONE);
-
-(*EPICS Motor record Update*)
-UpdateEpicsStatus();
-(*
-    Error from functions and Nc. The error will send to EPICS interface based on predifined
-    priority: axis, power, backlash, absoluteMove, etc...
-*)
-IF fbMcPower.Error AND fbMcPower.Active THEN
-    stMotionStage.bError:=fbMcPower.Error;
-    stMotionStage.nErrorId:=fbMcPower.ErrorID;
-ELSIF fbMcMoveAbsolute.Error THEN
-    stMotionStage.bError:=fbMcMoveAbsolute.Error;
-    stMotionStage.nErrorId:=fbMcMoveAbsolute.ErrorID;
-ELSIF fbMcHalt.Error AND fbMcHalt.Active THEN
-    stMotionStage.bError:=fbMcHalt.Error;
-    stMotionStage.nErrorId:=fbMcHalt.ErrorID;
-ELSIF fbMcReset.Error  THEN
-    stMotionStage.bError:=fbMcReset.Error;
-    stMotionStage.nErrorId:=fbMcReset.ErrorID;
-ELSIF fbMcWriteParameter.Error THEN
-    stMotionStage.bError:=fbMcWriteParameter.Error;
-    stMotionStage.nErrorId:=fbMcWriteParameter.ErrorID;
-ELSIF fbMcReadParams.Error AND NOT stMotionStage.bBusy THEN
-    stMotionStage.bError:=fbMcReadParams.Error;
-    stMotionStage.nErrorId:=fbMcReadParams.ErrorID;
-ELSE
-    IF stMotionStage.bBusy THEN
-        stMotionStage.sErrorMessage := '';
-        stMotionStage.sCustomErrorMessage := '';
-    END_IF
-END_IF;
-
-(*Double function, prioritize NC error otherwise read drive channel error code*)
-ReadDriveCodes();
-
-IF stMotionStage.sCustomErrorMessage <> ''
-    AND stMotionStage.sErrorMessage = '' THEN
-    stMotionStage.sErrorMessage := stMotionStage.sCustomErrorMessage;
-END_IF
-
-(*Clear motion flag when error occurs*)
-IF stMotionStage.bError  THEN
-    stMotionStage.bBusy := FALSE;
-    stMotionStage.bDone := FALSE;
-    stMotionStage.bEnable := FALSE;
-END_IF
-
-(* We've got the rising edge clear this flags.*)
-stMotionStage.bMoveCmd:=FALSE;
-stMotionStage.bHomeCmd:=FALSE;
-]]></ST>
-    </Implementation>
-    <Method Name="ExposedParameters" Id="{bfdcb735-2f35-43ca-9b4a-eca418143c19}">
-      <Declaration><![CDATA[METHOD PUBLIC  ExposedParameters
-VAR_INPUT
-    Enable : BOOL;
-    tRefreshDelay: TIME;
-END_VAR
-
-VAR_INST
-    {attribute 'hide'}
-    bParamReadTimer 	: BOOL := TRUE;
-END_VAR
-
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[// Periodic update of NC parameters
-NCParamsTimer( IN:=Enable AND NOT bExecParamsRead, PT:=tRefreshDelay);
-bExecParamsRead S= NCParamsTimer.Q;
-fbMcReadParams(
-    Parameter:=stMotionStage.stAxisParameters,
-    Axis:=stMotionStage.Axis,
-    Execute:= bExecParamsRead );
-
-// Copy axis parameters that we want to expose to the EPICS layer.
-stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl     := stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl;
-stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl   := stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl;
-stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl   := stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl;
-stMotionStage.stAxisParametersExposed.fAccelerationMax              := stMotionStage.stAxisParameters.fAccelerationMax;
-stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax               := stMotionStage.stAxisParameters.fCtrlPosDiffMax;
-stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime           := stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime;
-stMotionStage.stAxisParametersExposed.fDecelerationMax              := stMotionStage.stAxisParameters.fDecelerationMax;
-stMotionStage.stAxisParametersExposed.fEncSoftEndMax                := stMotionStage.stAxisParameters.fEncSoftEndMax;
-stMotionStage.stAxisParametersExposed.fEncSoftEndMin                := stMotionStage.stAxisParameters.fEncSoftEndMin;
-stMotionStage.stAxisParametersExposed.fVeloMaximum                  := stMotionStage.stAxisParameters.fVeloMaximum;
-stMotionStage.stAxisParametersExposed.fEncOffset               		:= stMotionStage.stAxisParameters.fEncOffset;
-stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal       := stMotionStage.stAxisParameters.fEncScaleFactorInternal;
-
-bExecParamsRead R= fbMcReadParams.Done OR fbMcReadParams.Error;
-stMotionStage.bAxisParamsInit S= fbMcReadParams.Done;]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="FB_Init" Id="{a8134580-303c-4b2f-8942-be52670189d3}">
-      <Declaration><![CDATA[//FB_Init is always available implicitly and it is used primarily for initialization.
-//The return value is not evaluated. For a specific influence, you can also declare the
-//methods explicitly and provide additional code there with the standard initialization
-//code. You can evaluate the return value.
-METHOD FB_Init: BOOL
-VAR_INPUT
-    bInitRetains: BOOL; // TRUE: the retain variables are initialized (reset warm / reset cold)
-    bInCopyCode: BOOL;  // TRUE: the instance will be copied to the copy code afterward (online change)
-    stPiezoStage		:	REFERENCE TO ST_MotionStage;
-    (*Mandatory must be unique for each MCS2 axis *)
-    eModule : E_Module;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[THIS^.stMotionStage REF= stPiezoStage;
-THIS^.eModule := eModule;
-
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="Halt" Id="{6287ad75-ad53-4d78-9e75-75537e8950c5}">
-      <Declaration><![CDATA[METHOD Halt : BOOL
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[fbMcHalt(
-    Axis := stMotionStage.Axis,
-    Execute := bStop AND bCSPModeEnabled,
-    Deceleration := stMotionStage.fDeceleration,
-    BufferMode := eBufferMode,
-);
-
-IF fbMcHalt.Done (*OR fbMcReset.Done*) THEN
-    (*Hold the position in close loop before dropping power, a positional drift will occur then*)
-    IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-        bPositionHold := TRUE;
-        tonHoldTime.IN:=TRUE;
-    END_IF
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="Home" Id="{30e09e6f-3e19-461e-88df-b1cd53b15969}">
-      <Declaration><![CDATA[METHOD PUBLIC Home
-
-VAR_INST
-    {attribute 'hide'}
-    bMove 				: BOOL;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[tonSyncHoming(PT:=T#20MS);
-CASE eHomeMode OF
-    E_EpicsHomeCmd.AUTOZERO:
-        bMove S=  bExecHome;
-    E_EpicsHomeCmd.NONE:
-        bMove := FALSE;
-        bHomeBusy :=  bExecHome;
-        bHomeDone := NOT  bExecHome;
-    ELSE
-        bMove := FALSE;
-END_CASE
-
-IF bMove THEN
-    CASE eHomeState OF
-        // Wait for a rising edge
-        E_MoveState.IDLING:
-            IF bExecHome AND bHomeModeEnabled THEN
-                bHomeDone := FALSE;
-                bHomeBusy := TRUE;
-                stMotionStage.bDone := FALSE;
-                tonSyncHoming.IN := TRUE;
-                stDS402Drive.nDS402DriveControl :=15;
-                eHomeState := E_MoveState.INIT;
-            END_IF
-        E_MoveState.INIT:
-            IF tonSyncHoming.Q THEN
-                tonSyncHoming.IN := FALSE;
-                stDS402Drive.nDS402DriveControl :=31;
-                eHomeState:=E_MoveState.STARTED;
-            END_IF
-        E_MoveState.STARTED :
-            // this is a comfirmation that routine is ongoing
-            IF NOT stDS402Drive.stDS402DriveStatus.TargetReached
-                AND NOT stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific THEN
-                bHomeBusy:=TRUE;
-                bHomeDone:=FALSE;
-                eHomeState:=E_MoveState.IN_PROGRESS;
-            END_IF
-
-        E_MoveState.IN_PROGRESS :
-
-        IF bStop THEN
-          stDS402Drive.nDS402DriveControl.8 :=1;
-          bHomeAborted := TRUE;
-          eHomeState:=E_MoveState.INTERRUPTED;
-
-        // Genral Motion error i.e Following error ?
-        ELSIF stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific
-                OR stDS402Drive.stDS402DriveStatus.WarningCondition THEN
-            stMotionStage.bError := TRUE;
-            eHomeState:=E_MoveState.ERROR;
-
-        // Reached
-        ELSIF stDS402Drive.stDS402DriveStatus.TargetReached
-            AND stDS402Drive.stDS402DriveStatus.Bit12_OpModeSpecific
-            AND NOT stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific THEN
-           eHomeState:=E_MoveState.DONE;
-        END_IF
-        E_MoveState.INTERRUPTED:
-            IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-                bPositionHold := TRUE;
-                tonHoldTime.IN:=TRUE;
-            END_IF
-            bHomeBusy:=FALSE;
-            eHomeState:=E_MoveState.IDLING;
-        E_MoveState.DONE:
-            bHomeBusy:=FALSE;
-            tonSyncHoming.IN := FALSE;
-            IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-                bPositionHold := TRUE;
-                tonHoldTime.IN:=TRUE;
-            END_IF
-            bHomeDone:=TRUE;
-            stMotionStage.fPosition:=0.0;
-            //stDS402Drive.nDS402DriveControl := 15;
-            eHomeState:=E_MoveState.IDLING;
-        E_MoveState.ERROR:
-            bHomeDone:=FALSE;
-            bHomeBusy:= FALSE;
-            IF bWrongParameter THEN
-                stMotionStage.sCustomErrorMessage:='Invalid motion Parameters';
-                bWrongParameter := FALSE;
-                stMotionStage.bError := TRUE;
-            END_IF
-            IF NOT stMotionStage.bError THEN
-                eHomeState:=E_MoveState.IDLING;
-            END_IF
-    END_CASE
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="Interlock" Id="{b0516fc0-3ac5-4a37-a4b7-b21194f9b805}">
-      <Declaration><![CDATA[METHOD PUBLIC Interlock
-
-VAR_INST
-    {attribute 'hide'}
-    bPositiveDirection  : BOOL;
-    {attribute 'hide'}
-    bNegativeDirection  : BOOL;
-    {attribute 'hide'}
-    ftForwardEnabled  	: F_TRIG;
-
-    {attribute 'hide'}
-    ftBackwardEnabled 	: F_TRIG;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[// Positive or Negative direction
-bPositiveDirection:=stMotionStage.bBusy AND stMotionStage.fPosDiff > 0;
-bNegativeDirection:=stMotionStage.bBusy AND stMotionStage.fPosDiff < 0;
-
-// For SmartAct MCS2 these limit are range and enstop limits. however a single bit in the status is used
-// bLimOverride  : act as an override after limit was hit to allow reverse movement
-stMotionStage.bLimitForwardEnable :=  bLimOverride OR NOT( bPositiveDirection AND stDS402Drive.stDS402DriveStatus.InternalLimitActive );
-stMotionStage.bLimitBackwardEnable := bLimOverride OR NOT( bNegativeDirection AND stDS402Drive.stDS402DriveStatus.InternalLimitActive );
-
-// use falling trigger to avoid spaming  sErrorMessage
-ftForwardEnabled(CLK:=stMotionStage.bLimitForwardEnable);
-ftBackwardEnabled(CLK:=stMotionStage.bLimitBackwardEnable);
-
-IF  NOT bHomeBusy AND ftForwardEnabled.Q THEN
-    // Not an error, just a warning
-    stMotionStage.sCustomErrorMessage:='Cannot move past positive limit.';
-    IF NOT bStepModeEnabled THEN
-        bLimOverride := TRUE;
-        bStop := TRUE;
-    END_IF
-END_IF
-
-IF NOT bHomeBusy AND ftBackwardEnabled.Q THEN
-    // Not an error, just a warning
-    stMotionStage.sCustomErrorMessage:='Cannot move past Negative limit.';
-    IF NOT bStepModeEnabled THEN
-        bLimOverride := TRUE;
-        bStop := TRUE;
-    END_IF
-END_IF
-
-IF NOT stMotionStage.bError AND stMotionStage.bExecute AND NOT stMotionStage.bUserEnable THEN
-    stMotionStage.bError := TRUE;
-    stMotionStage.nErrorId := 1;
-    stMotionStage.sCustomErrorMessage := 'Move requested, but user enable is disabled!';
-END_IF
-
-// Update all enable booleans
-SetEnables();]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ModeOperation" Id="{101f52ed-fac5-4fd9-b68c-696a0d98bf98}">
-      <Declaration><![CDATA[(*Switch the drive mod eof Operation
-  Servo On: Profil Positioning or HOME
-  Servo Off : STEP mode
-*)
-METHOD ModeOperation
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[rtCSPModeOk(CLK:=( stDS402Drive.nModeOfOperationDisplay=E_DS402OpMode.CSP));
-rtHomeModeEnable(CLK:=( stDS402Drive.nModeOfOperationDisplay=E_DS402OpMode.HOME));
-rtStepModeEnabled(CLK:=( stDS402Drive.nModeOfOperationDisplay=E_DS402OpMode.MCS2_OL_STEP_MODE));
-// transition to close loop mode
-IF rtCSPModeOk.Q THEN
-    bCSPModeEnabled := TRUE;
-ELSIF rtHomeModeEnable.Q THEN
-    bHomeModeEnabled := TRUE;
-ELSIF rtStepModeEnabled.Q THEN
-    bStepModeEnabled := TRUE;
-    bServo := FALSE;
-END_IF
-
-IF rtUserExec.Q AND stMotionStage.bHomeCmd THEN
-    IF (stDS402Drive.nModeOfOperationDisplay<>E_DS402OpMode.HOME) THEN
-        stDS402Drive.nModeOfOperation := E_DS402OpMode.HOME;
-        IF stDS402Drive.nDS402DriveControl.4 THEN
-            stDS402Drive.nDS402DriveControl := 15;
-        ELSE
-            stDS402Drive.nDS402DriveControl := 128;
-        END_IF
-        bManualTransition := TRUE;
-        // clear previous operational from switching away from this mode.
-        bOperational := FALSE;
-        bStepModeEnabled := FALSE;
-        bCSPModeEnabled := FALSE;
-    ELSE
-        bHomeModeEnabled := TRUE;
-    END_IF
-ELSIF rtUserExec.Q AND bServo THEN
-    IF (stDS402Drive.nModeOfOperationDisplay<>E_DS402OpMode.CSP) THEN
-        stDS402Drive.nModeOfOperation := E_DS402OpMode.CSP;
-        IF stDS402Drive.nDS402DriveControl.4 THEN
-            stDS402Drive.nDS402DriveControl := 15;
-        ELSE
-            stDS402Drive.nDS402DriveControl := 128;
-        END_IF
-        bManualTransition := FALSE;
-        bHomeModeEnabled := FALSE;
-        bStepModeEnabled := FALSE;
-    ELSE
-        bCSPModeEnabled := TRUE;
-    END_IF
-ELSIF rtUserExec.Q AND NOT bServo THEN
-    IF (stDS402Drive.nModeOfOperationDisplay<>E_DS402OpMode.MCS2_OL_STEP_MODE) THEN
-        stDS402Drive.nModeOfOperation := E_DS402OpMode.MCS2_OL_STEP_MODE;
-        stDS402Drive.nDS402DriveControl := 128;
-        // clear previous operational from switching away from this mode.
-        bOperational := FALSE;
-        bCSPModeEnabled := FALSE;
-        bHomeModeEnabled := FALSE;
-    ELSE
-        bStepModeEnabled := TRUE;
-    END_IF
-    bManualTransition := TRUE;
-END_IF
-
-bDS402ManualEnabled :=bHomeModeEnabled OR bStepModeEnabled;
-(*Handle correct startup procedure for manual move*)
-StateMachine(Enable:=1);]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="MoveAbsolute" Id="{c9986ba7-a025-4f1a-8312-93dc4112a24c}">
-      <Declaration><![CDATA[METHOD PUBLIC  MoveAbsolute
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[
-fbMcMoveAbsolute(
-    Axis := stMotionStage.Axis,
-    Execute := bCommandMoveAbsolute,
-    Position := stMotionStage.fPosition,
-    Velocity := stMotionStage.fVelocity,
-    Acceleration := stMotionStage.fAcceleration,
-    Deceleration := stMotionStage.fDeceleration,
-    BufferMode := eBufferMode,
-);
-
-
-IF fbMcMoveAbsolute.Done (*OR fbMcReset.Done*) THEN
-    (*Hold the position in close loop before dropping power, a positional drift will occur then*)
-    IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-        bPositionHold := TRUE;
-        tonHoldTime.IN:=TRUE;
-    END_IF
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="PersistParameters" Id="{827e19b0-eb90-45f1-a0f0-e397f2f834ae}">
-      <Declaration><![CDATA[METHOD PersistParameters
-VAR_INPUT
-    Enable	: BOOL;
-END_VAR
-
-VAR_INST
-    {attribute 'hide'}
-    bEncError			: BOOL;
-
-    {attribute 'hide'}
-    bRestoreWaitRetry	: BOOL;
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[// Save Encoder position
-// needs some pre-conditions to use persistent storage
-IF Enable THEN
-    // Check ST_MotionStage for an encoder error (range 0x44nn)
-    bEncError:= stMotionStage.Axis.Status.Error AND  ( stMotionStage.Axis.Status.ErrorID  >= 16#4400  AND stMotionStage.Axis.Status.ErrorID <= 16#44FF );
-    // Do not save if we're currently loading or if there is an encoder error
-    IF NOT bRestoreLoad AND NOT bEncError AND NOT bRestoreWaitRetry THEN
-        fSavedPosition := stMotionStage.Axis.NcToPlc.ActPos;
-        // This persistent variable lets us check if anything was saved
-        // It will be TRUE at startup if we have saved values
-        bSaved := TRUE;
-        (*
-            use this with a timer of a change threshold on the fActPosition
-            to trigger a save from this PLC program. otherwise this will be a spamming
-            persistence, filling up the disk.
-        *)
-        //iPersistentDataStorage.TriggerWriteOfPersistentData := bSaveEnabled;
-    END_IF
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="PostInit" Id="{7af64fe1-72e2-4a67-9fd4-3933db27c0b4}">
-      <Declaration><![CDATA[{attribute 'call_after_init'}
-(*	eModule: Mandatory must be unique for each MCS2 axis
-    clears startup errors or warnings. the process is completed at run time
-    SDO update of Drive and open Loop parameter Indexes based on module number
-*)
-METHOD PostInit
-VAR_INPUT
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[(* SDO update of Drive and open Loop parameter Indexes based on module number*)
-nChanErrorCodeIdx	:= nErrorCodeBase		+ ((eModule-1) * nChanOfsBase);
-nChanFErrWinIdx		:= nFErrWinBase 		+ ((eModule-1) * nChanOfsBase);
-nChanSoftLimitIdx	:= nChanSoftLimitBase 	+ ((eModule-1) * nChanOfsBase);
-nChanHomeOffsIdx 	:= nHomeOffsetBase 		+ ((eModule-1) * nChanOfsBase);
-nChanHomeVeloIdx 	:= nHomingSpeedBase	 	+ ((eModule-1) * nChanOfsBase);
-nChanhomeAccIdx 	:= nhomingAccBase 		+ ((eModule-1) * nChanOfsBase);
-nChanStepIdx    	:= nOlStepBase 			+ ((eModule-1) * nChanOfsBase);
-nChanStepFreqIdx  := nOlStepFreqBase 		+ ((eModule-1) * nChanOfsBase);
-nChanStepAmpIdx 	:= nOlStepAmpBase 		+ ((eModule-1) * nChanOfsBase);
-bSystemInit := TRUE;
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="Power" Id="{f641a677-a472-4a34-a0e0-f26f3662273e}">
-      <Declaration><![CDATA[METHOD Power : BOOL
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[fbMcPower(
-    Axis := stMotionStage.Axis,
-    Enable := stMotionStage.bAllEnable AND bCSPModeEnabled,
-    Enable_Positive := stMotionStage.bAllForwardEnable,
-    Enable_Negative := stMotionStage.bAllBackwardEnable,
-    Override := 100.0,
-    BufferMode := eBufferMode );]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ReadDriveCodes" Id="{c8b026c7-a02a-444a-a6c8-2fcd1b30b9c5}">
-      <Declaration><![CDATA[(* Read Drive error codes after a motion or fault condition occured *)
-METHOD ReadDriveCodes : BOOL
-VAR_INST
-    {attribute 'hide'}
-    fbErrorRead			: FB_EcCoESdoRead;
-    {attribute 'hide'}
-    nPiezoErrorCode   	: UDINT;
-    {attribute 'hide'}
-    fbLogError  		: FB_LogMotionError;
-    {attribute 'hide'}
-    ftErrorReadDone  	: F_TRIG;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[fbErrorRead( sNetId:= THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-             nSlaveAddr:=stDS402Drive.nSlaveAddr,
-             nIndex:=nChanErrorCodeIdx,
-             nSubIndex :=nSubIndex,
-             pDstBuf:= ADR(nPiezoErrorCode),
-             cbBufLen:=SIZEOF(nPiezoErrorCode),
-             bExecute:= stMotionStage.bError );
-
-ftErrorReadDone(CLK:=fbErrorRead.bBusy);
-IF ftErrorReadDone.Q THEN
-    IF 	NOT fbErrorRead.bError AND nPiezoErrorCode <> 0 THEN
-        stMotionStage.nErrorId := nPiezoErrorCode;
-    END_IF
-    stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId:=stMotionStage.nErrorId);
-    fbLogError( stMotionStage:=stMotionStage, bEnable:=stMotionStage.bError);
-END_IF
-
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="Reset" Id="{643b4736-0a43-4a18-8f6b-8a2439ed5e6f}">
-      <Declaration><![CDATA[METHOD Reset : BOOL
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[fbMcReset(
-    Axis := stMotionStage.Axis,
-    Execute := bLocalReset AND bCSPModeEnabled
-);
-
-IF fbMcReset.Done OR fbMcReset.Error THEN
-    bLocalReset := FALSE;
-    IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-        stMotionStage.bEnable:=FALSE;
-    END_IF
-
-END_IF
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="RestoreMotionParams" Id="{8335d215-fac8-43bf-97f2-e08acc164700}">
-      <Declaration><![CDATA[METHOD RestoreMotionParams
-VAR_INPUT
-    Enable	: BOOL;
-END_VAR
-
-VAR_INST
-    {attribute 'hide'}
-    fbSetPos			: MC_SetPosition;
-    {attribute 'hide'}
-    bLoad				: BOOL;
-    {attribute 'hide'}
-    bRestoreDone 		: BOOL;
-    {attribute 'hide'}
-    bRestoreInit		: BOOL;
-    {attribute 'hide'}
-    bRestoreWaitRetry	: BOOL;
-    {attribute 'hide'}
-    nMaxRetries			: UINT := 10;
-    {attribute 'hide'}
-    nCurrTries			: UINT := 0;
-    {attribute 'hide'}
-    nLatchError			: UDINT;
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[IF Enable THEN
-    // Trigger a load if anything was saved at all
-    IF NOT bRestoreInit THEN
-        bRestoreInit := TRUE;
-        bRestoreLoad S= bSaved;
-        fbSetPos.Options.ClearPositionLag := TRUE;
-    END_IF
-
-    // Set our position if bRestoreLoad is true
-    fbSetPos( Axis:=stMotionStage.Axis, Execute:=bRestoreLoad, Position:=fSavedPosition);
-
-    // Only load once, at startup
-    bRestoreLoad R= fbSetPos.Done OR fbSetPos.Error;
-
-    IF fbSetPos.Error THEN
-        // Keep the error latched, it can disappear if Execute is set to FALSE
-        nLatchError := fbSetPos.ErrorID;
-        nCurrTries := nCurrTries + 1;
-        IF nCurrTries >= nMaxRetries THEN
-        // Alert the user that something has gone wrong
-        stMotionStage.bError := TRUE;
-        stMotionStage.nErrorId := nLatchError;
-        stMotionStage.sCustomErrorMessage := 'Error loading previously saved position.';
-        ELSE
-            // Reset the FB for the next retry
-            fbSetPos( Axis:=stMotionStage.Axis, Execute:=bRestoreLoad, Position:=fSavedPosition);
-            // Try again
-            bRestoreWaitRetry := TRUE;
-        END_IF
-    ELSE
-        IF NOT bRestoreDone THEN
-            stMotionStage.fPosition := fSavedPosition;
-        END_IF
-        bRestoreDone := TRUE;
-    END_IF
-
-    tonRestoreRetry( IN := bRestoreWaitRetry, PT := T#100MS);
-    bRestoreLoad S= tonRestoreRetry.Q;
-    bRestoreWaitRetry R= tonRestoreRetry.Q;
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ScaleDriveParams" Id="{b413f5fa-ba01-446e-8e58-be0ddf41b7f7}">
-      <Declaration><![CDATA[METHOD ScaleDriveParams
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[nScaledFErrWin:=LREAL_TO_DINT((stMotionStage.stAxisParameters.fCtrlPosDiffMax / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-
-IF stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl.0 AND stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl.0 THEN
-    nSoftLimMax := LREAL_TO_DINT((stMotionStage.stAxisParameters.fEncSoftEndMax / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-    nSoftLimMin := LREAL_TO_DINT((stMotionStage.stAxisParameters.fEncSoftEndMin / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-ELSE
-    // Soft limit disabled
-    nSoftLimMax := 0;
-    nSoftLimMin := 0;
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ScaleEncRawValue" Id="{769f98a6-388a-461f-854a-93b1758d578f}">
-      <Declaration><![CDATA[METHOD ScaleEncRawValue
-VAR_INPUT
-  //  bEnable: BOOL;
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[(* SmartACT: The encoder count from the Piezo drive is not a raw count from the embeded actuator device
-   we get a calibrated nm value. thus the use of abs() since this value is signed.
-   PI: to be discovered *)
-IF stMotionStage.nRawEncoderDINT <> 0 THEN
-    stMotionStage.nEncoderCount:=DINT_TO_UDINT(ABS(stMotionStage.nRawEncoderDINT));
-ELSE
-    stMotionStage.nEncoderCount:=0;
-END_IF
-
-// calibrated encoder readback
-IF NOT bStepModeEnabled THEN
-    // Close loop NC
-    fMeasuredPos:=stMotionStage.Axis.NcToPlc.ActPos;
-    fMeasuredVelo :=stMotionStage.Axis.NcToPlc.ActVelo;
-    fMeasuredAcc := stMotionStage.Axis.NcToPlc.ActAcc;
-    stMotionStage.fPosDiff:=stMotionStage.Axis.NcToPlc.PosDiff;
-ELSE
-    fMeasuredPos:=DINT_TO_REAL(stMotionStage.nRawEncoderDINT) * MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor);
-    stMotionStage.fPosDiff:=stMotionStage.fPosition - fMeasuredPos;
-    // NB: Not actual in open loop
-    fMeasuredVelo :=stMotionStage.fVelocity;
-    fMeasuredAcc := stMotionStage.fAcceleration;
-END_IF
-
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ScaleHomeMotionParams" Id="{2c5b56b0-7eaa-4a13-8084-de781b4be994}">
-      <Declaration><![CDATA[(*Scale Home Move parameters*)
-METHOD ScaleHomeMotionParams
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[nHomeVeloFast := LREAL_TO_UDINT((stMotionStage.stAxisParameters.fRefVeloSearch / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-nHomeVeloSlow := LREAL_TO_UDINT((stMotionStage.stAxisParameters.fRefVeloSync / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-nHomeAcc := LREAL_TO_UDINT((THIS^.fHomeAcc / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-nHomeOffset := LREAL_TO_DINT((stMotionStage.fHomePosition / MAX(stMotionStage.stAxisParameters.fEncScaleFactorInternal, fScalingFactor)));
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="ScaleServoOffMotionParams" Id="{78ce5a79-ef3b-44af-a794-7ac04154475b}">
-      <Declaration><![CDATA[(*Scaled Open Loop Step mode motion parameters*)
-METHOD ScaleServoOffMotionParams
-VAR_INPUT
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[IF 	nChanStepAmp = 0
-    OR nChanStepFreq = 0 THEN
-    bWrongParameter := TRUE;
-ELSE
-    fScalededSteps := (THIS^.stMotionStage.fPosition * nChanStepEgu) ;
-    nScalededSteps:=LIMIT(-1000, LREAL_TO_DINT(fScalededSteps), 1000);
-    nScalededStepAmp:=REAL_TO_UINT(LIMIT(50, nChanStepAmp, 100) * 655.35);
-    nScaledStepFreq:=REAL_TO_UINT(LIMIT(500, nChanStepFreq, 1000.0));
-    bWrongParameter := FALSE;
-END_IF
-//827.31x-837.63]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="SetEnables" Id="{28f7e94e-bad4-40ca-b34a-f8da453343cc}">
-      <Declaration><![CDATA[METHOD SetEnables
-VAR_INPUT
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[stMotionStage.bAllForwardEnable:=stMotionStage.bLimitForwardEnable AND (stMotionStage.bGantryForwardEnable OR NOT stMotionStage.bGantryAxis) AND stMotionStage.stEPSForwardEnable.bEPS_OK;
-stMotionStage.bAllBackwardEnable:=stMotionStage.bLimitBackwardEnable AND (stMotionStage.bGantryBackwardEnable OR NOT stMotionStage.bGantryAxis) AND stMotionStage.stEPSBackwardEnable.bEPS_OK;
-
-stMotionStage.bAllEnable:=stMotionStage.bEnable AND stMotionStage.bHardwareEnable AND stMotionStage.stEPSPowerEnable.bEPS_OK;
-stMotionStage.bAllEnable R= NOT stMotionStage.bUserEnable;]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="StateMachine" Id="{08b990dd-982c-4d9b-9ed0-d47355966885}">
-      <Declaration><![CDATA[METHOD StateMachine
-VAR_INPUT
-    Enable : BOOL;
-END_VAR
-
-VAR_INST
-    rtFault: R_TRIG;
-    {attribute 'hide'}
-    bFault				: BOOL;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[(*switch on disable*)
-IF NOT stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND  stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
-        bOperational := FALSE;
-        bFault := FALSE;
-        IF bManualTransition THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 6;
-        END_IF
-(*Ready to switch*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
-        bFault := FALSE;
-        bOperational := FALSE;
-        IF bManualTransition THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 7;
-        END_IF
-(*Switch on*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND  stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
-        bOperational := FALSE;
-        IF bManualTransition THEN
-            THIS^.stDS402Drive.nDS402DriveControl := 15;
-        END_IF
-(*Operation enbaled*)
-ELSIF stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
-        THIS^.bOperational := TRUE;
-(*Fault*)
-ELSIF  NOT stDS402Drive.stDS402DriveStatus.ReadyToSwitchOn
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchedOn
-    AND NOT stDS402Drive.stDS402DriveStatus.OperationEnabled
-    AND stDS402Drive.stDS402DriveStatus.Fault
-    AND stDS402Drive.stDS402DriveStatus.VoltageEnabled
-    AND NOT stDS402Drive.stDS402DriveStatus.QuickStopActive
-    AND NOT stDS402Drive.stDS402DriveStatus.SwitchOnDisabled THEN
-        bOperational := FALSE;
-        bFault := TRUE;
-
-ELSE
-    bFault := FALSE;
-    bOperational := FALSE;
-END_IF
-
-// wait for parameter init done at startup to read potential error from drive
-stMotionStage.bError := bFault AND stMotionStage.bAxisParamsInit;
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="StepMove" Id="{74869c9c-654c-4a5a-9a50-bfad55265981}">
-      <Declaration><![CDATA[METHOD  StepMove
-VAR_INPUT
-    Enable : BOOL;
-END_VAR]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[rtStepMove(CLK:=Enable);
-CASE eMotionState OF
-    E_MoveState.IDLING:;
-        // Motion states
-        IF rtStepMove.Q THEN
-            // enter move discrete or continous here:
-            bStepMoveBusy := TRUE;
-            bStepMoveDone := FALSE;
-            stMotionStage.bDone:=FALSE;
-            bSteModeEnable := FALSE;
-            stDS402Drive.nDS402DriveControl:=15;
-            eMotionState:=E_MoveState.INIT;
-        END_IF
-    E_MoveState.INIT:
-            stDS402Drive.nDS402DriveControl:=31;
-            IF stMotionStage.bError THEN
-                eMotionState:=E_MoveState.ERROR;
-            ELSE
-                stDS402Drive.nDS402DriveControl:=31;
-                  eMotionState:=E_MoveState.IN_PROGRESS;
-            END_IF
-
-    E_MoveState.IN_PROGRESS:
-        IF bStop THEN
-            stDS402Drive.nDS402DriveControl.8 :=1;
-            bStepMoveAborted := TRUE;
-            eMotionState:=E_MoveState.INTERRUPTED;
-        ELSIF stDS402Drive.stDS402DriveStatus.Bit13_OpModeSpecific THEN
-            stMotionStage.bError := TRUE;
-            eMotionState:=E_MoveState.ERROR;
-        ELSE
-            // Reached ?
-            IF stDS402Drive.stDS402DriveStatus.TargetReached
-                 AND NOT stDS402Drive.stDS402DriveStatus.Bit12_OpModeSpecific THEN
-                eMotionState:=E_MoveState.REACHED;
-            END_IF
-        END_IF
-    E_MoveState.INTERRUPTED:
-        IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-            bPositionHold := TRUE;
-            tonHoldTime.IN:=TRUE;
-        END_IF
-        bStepMoveBusy:=FALSE;
-        eMotionState:=E_MoveState.IDLING;
-    E_MoveState.REACHED:
-        // Release the endstop override
-        bLimOverride := FALSE;
-        bStepMoveBusy:=FALSE;
-        IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
-            bPositionHold := TRUE;
-            tonHoldTime.IN:=TRUE;
-        END_IF
-        bStepMoveDone:=TRUE;
-        stDS402Drive.nDS402DriveControl:=15;
-        IF stMotionStage.bDone THEN
-            eMotionState:=E_MoveState.IDLING;
-        END_IF
-    E_MoveState.ERROR:
-        bStepMoveBusy:=FALSE;
-        bStepMoveDone:=FALSE;
-        IF bWrongParameter THEN
-            stMotionStage.sCustomErrorMessage:='Invalid motion Parameters';
-            bWrongParameter := FALSE;
-            stMotionStage.bError := TRUE;
-        END_IF
-        IF NOT stMotionStage.bError THEN
-            eMotionState:=E_MoveState.IDLING;
-        END_IF
-END_CASE]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="UpdateDriveMonitoringParams" Id="{bba7eb59-9b08-4212-8ebb-7515fbf84d2c}">
-      <Declaration><![CDATA[(*	Drive parameters update
-    Mirror NC axis parameters configurations to those of MCS2 drive
-    1- Following error
-    2- Home Speeds
-    3- Home Acceleration
-    4- SoftLimits
-*)
-METHOD UpdateDriveMonitoringParams
-VAR_INPUT
-    Enable : BOOL;
-END_VAR
-
-VAR_INST
-    {attribute 'hide'}
-    fbSetFErrorWin		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetSoftLimMin		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetSoftLimMax		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetHomeOffs 		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetHomeVeloFast 	: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetHomeVeloSlow 	: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetHomeAcc		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    nRecentSoftLimMax 	: LINT;
-    {attribute 'hide'}
-    nRecentSoftLimMin 	: LINT;
-    {attribute 'hide'}
-    nRecentHomeVeloFast : UDINT;
-    {attribute 'hide'}
-    nRecentHomeVeloSlow : UDINT;
-    {attribute 'hide'}
-    nRecentHomeAcc 		: UDINT;
-    {attribute 'hide'}
-    nRecentFErrWin		: DINT;
-    {attribute 'hide'}
-    nRecentHomeOffset 	: DINT;
-    {attribute 'hide'}
-    nHomeOffs			: DINT;
-    {attribute 'hide'}
-    ftFErrWinSetDone	: F_TRIG;
-
-    {attribute 'hide'}
-    ftSoftLimMaxSetDone	: F_TRIG;
-    {attribute 'hide'}
-    ftSoftLimMinSetDone	: F_TRIG;
-    {attribute 'hide'}
-    ftHomeVeloFastSetDone: F_TRIG;
-    {attribute 'hide'}
-    ftHomeAccSetDone	: F_TRIG;
-    {attribute 'hide'}
-    ftHomeOffsSetDone	: F_TRIG;
-    {attribute 'hide'}
-    ftHomeVeloSlowSetDone: F_TRIG;
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[IF Enable THEN
-
-    (*	Drive parameters update
-        Mirror NC axis parameters configurations to those of MCS2 drive
-        1- Following error.
-        2- SoftLimits
-        3-Position Range
-    *)
-  // add some safety here for  0 div.
-  ScaleDriveParams();
-  ScaleHomeMotionParams ();
-
-  fbSetFErrorWin(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanFErrWinIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nScaledFErrWin),
-    cbBufLen   := SIZEOF(nScaledFErrWin),
-    bExecute   := NOT stMotionStage.bBusy AND (nScaledFErrWin <> nRecentFErrWin)
-  );
-  fbSetSoftLimMin(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanSoftLimitIdx,
-    nSubIndex  := 1,
-    pSrcBuf    := ADR(nSoftLimMin),
-    cbBufLen   := SIZEOF(nSoftLimMin),
-    bExecute   := NOT stMotionStage.bBusy AND (nSoftLimMin <> nRecentSoftLimMin)
-  );
-  fbSetSoftLimMax(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanSoftLimitIdx,
-    nSubIndex  := 2,
-    pSrcBuf    := ADR(nSoftLimMax),
-    cbBufLen   := SIZEOF(nSoftLimMax),
-    bExecute   := NOT stMotionStage.bBusy AND (nSoftLimMax <> nRecentSoftLimMax)
-    );
-  fbSetHomeVeloFast(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanHomeVeloIdx,
-    nSubIndex  := 1,
-    pSrcBuf    := ADR(nHomeVeloFast),
-    cbBufLen   := SIZEOF(nHomeVeloFast),
-    bExecute   := NOT stMotionStage.bBusy AND (nHomeVeloFast <> nRecentHomeVeloFast)
-  );
-  fbSetHomeVeloSlow(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanHomeVeloIdx,
-    nSubIndex  := 2,
-    pSrcBuf    := ADR(nHomeVeloSlow),
-    cbBufLen   := SIZEOF(nHomeVeloSlow),
-    bExecute   := NOT stMotionStage.bBusy AND (nHomeVeloSlow <> nRecentHomeVeloSlow)
-  );
-  fbSetHomeAcc(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanhomeAccIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nHomeAcc),
-    cbBufLen   := SIZEOF(nHomeAcc),
-    bExecute   := NOT stMotionStage.bBusy AND (nHomeAcc <> nRecentHomeAcc)
-  );
-
-  fbSetHomeOffs(
-    sNetId     := THIS^.stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanHomeOffsIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nHomeOffset),
-    cbBufLen   := SIZEOF(nHomeOffset),
-    bExecute   := NOT stMotionStage.bBusy AND (nHomeOffset <> nRecentHomeOffset)
-  );
-
-  ftFErrWinSetDone(CLK:=fbSetFErrorWin.bBusy);
-  ftSoftLimMaxSetDone(CLK:=fbSetSoftLimMax.bBusy);
-  ftSoftLimMinSetDone(CLK:=fbSetSoftLimMin.bBusy);
-  ftHomeVeloFastSetDone(CLK:=fbSetHomeVeloFast.bBusy);
-  ftHomeVeloSlowSetDone(CLK:=fbSetHomeVeloFast.bBusy);
-  ftHomeAccSetDone(CLK:=fbSetHomeAcc.bBusy);
-  ftHomeOffsSetDone(CLK:=fbSetHomeOffs.bBusy);
-
-    IF ftFErrWinSetDone.Q OR ftSoftLimMaxSetDone.Q
-        OR ftSoftLimMinSetDone.Q OR ftHomeVeloFastSetDone.Q
-        OR ftHomeAccSetDone.Q OR ftHomeOffsSetDone.Q OR ftHomeVeloSlowSetDone.Q THEN
-        IF fbSetFErrorWin.bError THEN
-           stMotionStage.bError := fbSetFErrorWin.bError;
-           stMotionStage.nErrorId := fbSetFErrorWin.nErrId;
-        ELSIF fbSetSoftLimMin.bError THEN
-           stMotionStage.bError := fbSetSoftLimMin.bError;
-           stMotionStage.nErrorId := fbSetSoftLimMin.nErrId;
-        ELSIF fbSetSoftLimMax.bError THEN
-           stMotionStage.bError := fbSetSoftLimMax.bError;
-           stMotionStage.nErrorId := fbSetSoftLimMax.nErrId;
-        ELSIF fbSetHomeVeloFast.bError THEN
-           stMotionStage.bError := fbSetHomeVeloFast.bError;
-           stMotionStage.nErrorId := fbSetHomeVeloFast.nErrId;
-        ELSIF fbSetHomeVeloSlow.bError THEN
-           stMotionStage.bError := fbSetHomeVeloSlow.bError;
-           stMotionStage.nErrorId := fbSetHomeVeloSlow.nErrId;
-        ELSIF fbSetHomeAcc.bError THEN
-           stMotionStage.bError := fbSetHomeAcc.bError;
-           stMotionStage.nErrorId := fbSetHomeAcc.nErrId;
-        ELSIF fbSetHomeOffs.bError THEN
-           stMotionStage.bError := fbSetHomeOffs.bError;
-           stMotionStage.nErrorId := fbSetHomeOffs.nErrId;
-        ELSE
-           nRecentFErrWin := nScaledFErrWin;
-           nRecentSoftLimMax := nSoftLimMax;
-           nRecentSoftLimMin := nSoftLimMin;
-           nRecentHomeVeloFast :=nHomeVeloFast;
-           nRecentHomeVeloSlow := nHomeVeloSlow;
-           nRecentHomeAcc := nHomeAcc;
-           nRecentHomeOffset := nHomeOffs;
-        END_IF
-        fbSetFErrorWin.bExecute := FALSE;
-        fbSetSoftLimMin.bExecute := FALSE;
-        fbSetSoftLimMax.bExecute := FALSE;
-        fbSetHomeVeloFast.bExecute := FALSE;
-        fbSetHomeVeloSlow.bExecute := FALSE;
-        fbSetHomeAcc.bExecute := FALSE;
-        fbSetHomeOffs.bExecute := FALSE;
-    END_IF
-END_IF]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="UpdateEpicsStatus" Id="{dc90c8f8-9d88-4ab9-9dc0-f90cd21b81fc}">
-      <Declaration><![CDATA[METHOD UpdateEpicsStatus
-VAR_INPUT
-END_VAR
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[//////////////////////////////////////////////////////////////////
-// Transfer NC parameter status to EPICS
-//////////////////////////////////////////////////////////////////
-stMotionStage.nMotionAxisID := stMotionStage.Axis.NcToPlc.AxisId;
-stMotionStage.stAxisStatus.bEnable:=stMotionStage.bAllEnable;
-stMotionStage.stAxisStatus.bEnabled:=stMotionStage.bEnableDone; // account for PowerEnable from power block
-stMotionStage.stAxisStatus.bError:=stMotionStage.bError;
-stMotionStage.stAxisStatus.bHomeSensor:=stMotionStage.bHome;
-stMotionStage.stAxisStatus.bLimitBwd:=stMotionStage.bAllBackwardEnable;
-stMotionStage.stAxisStatus.bLimitFwd:=stMotionStage.bAllForwardEnable;
-stMotionStage.stAxisStatus.bReset:=stMotionStage.bReset;
-stMotionStage.stAxisStatus.fAcceleration:=fMeasuredAcc;
-stMotionStage.stAxisStatus.fActDiff:=stMotionStage.fPosDiff;
-stMotionStage.stAxisStatus.fActPosition:=fMeasuredPos;
-stMotionStage.stAxisStatus.fActVelocity:=fMeasuredVelo;
-stMotionStage.stAxisStatus.fDeceleration:=fMeasuredAcc;
-stMotionStage.stAxisStatus.fOverride:=fbMcPower.Override;
-stMotionStage.stAxisStatus.fPosition:=stMotionStage.fPosition;
-stMotionStage.stAxisStatus.fVelocity:=stMotionStage.fVelocity;
-stMotionStage.stAxisStatus.nCmdData:=INT_TO_UINT(stMotionStage.nCmdData);  //Or nCmdDataLocal
-stMotionStage.stAxisStatus.nCommand:=INT_TO_UINT(stMotionStage.nCommand);  //Or nCommandLocal
-stMotionStage.stAxisStatus.nErrorId:=stMotionStage.nErrorId;
-stMotionStage.stAxisStatus.bBusy:=stMotionStage.bBusy;
-stMotionStage.stAxisStatus.bHomed:=stMotionStage.bHomed;
-stMotionStage.stAxisStatus.bExecute:=bLocalExec;
-stMotionStage.stAxisStatus.nCommand:=3; // If this is not 3, the IOC stops updating positions during homing
-
-
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="UpdateServoOffMotionParams" Id="{4e8302eb-f6c2-4802-bf37-2e40ea9fd85c}">
-      <Declaration><![CDATA[(*	MCS2 OL Step parameters update
-    1- Step
-    2- Step Amplitude
-    3- StepFreq
-*)
-METHOD UpdateServoOffMotionParams
-VAR_INPUT
-    Enable : BOOL;
-END_VAR
-
-VAR_INST
-    // Open Loop Operations
-    {attribute 'hide'}
-    fbSetSteps			: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetStepFreq 		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    fbSetStepAmp  		: FB_EcCoESdoWrite;
-    {attribute 'hide'}
-    nRecentSteps		: DINT;
-    {attribute 'hide'}
-    nRecentStepAmp		: UINT;
-    {attribute 'hide'}
-    nRecentStepFreq		: UINT;
-    {attribute 'hide'}
-    ftStepAmpUpdateDone	: F_TRIG;
-    {attribute 'hide'}
-    ftStepFreqUpdateDone	: F_TRIG;
-    {attribute 'hide'}
-    ftStepUpdateDone	: F_TRIG;
-END_VAR
-
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[IF Enable THEN
-  ScaleServoOffMotionParams();
-  fbSetSteps(
-    sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanStepIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nScalededSteps),
-    cbBufLen   := SIZEOF(nChanStep),
-    bExecute   := NOT stMotionStage.bBusy AND (nScalededSteps <> nRecentSteps)
-  );
-  fbSetStepAmp(
-    sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanStepAmpIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nScalededStepAmp),
-    cbBufLen   := SIZEOF(nChanStepAmp),
-    bExecute   := NOT stMotionStage.bBusy AND (nScalededStepAmp <> nRecentStepAmp)
-  );
-  fbSetStepFreq(
-    sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
-    nSlaveAddr := stDS402Drive.nSlaveAddr,
-    nIndex     := nChanStepFreqIdx,
-    nSubIndex  := 0,
-    pSrcBuf    := ADR(nScaledStepFreq),
-    cbBufLen   := SIZEOF(nChanStepFreq),
-    bExecute   := NOT stMotionStage.bBusy AND (nScaledStepFreq <> nRecentStepFreq)
-  );
-  ftStepUpdateDone(CLK:=fbSetSteps.bBusy);
-  ftStepAmpUpdateDone(CLK:=fbSetStepAmp.bBusy);
-  ftStepFreqUpdateDone(CLK:=fbSetStepFreq.bBusy);
-
-    bStepModeparamsSetDone R= fbSetSteps.bBusy OR fbSetStepAmp.bBusy OR fbSetStepFreq.bBusy;
-  IF ftStepUpdateDone.Q
-        OR ftStepAmpUpdateDone.Q
-        OR ftStepFreqUpdateDone.Q THEN
-    IF fbSetSteps.bError THEN
-            stMotionStage.bError := fbSetSteps.bError;
-            stMotionStage.nErrorId := fbSetSteps.nErrId;
-        ELSIF fbSetStepFreq.bError THEN
-                stMotionStage.bError := fbSetStepFreq.bError;
-                stMotionStage.nErrorId := fbSetStepFreq.nErrId;
-        ELSIF fbSetStepAmp.bError THEN
-                stMotionStage.bError := fbSetStepAmp.bError;
-                stMotionStage.nErrorId := fbSetStepAmp.nErrId;
-    ELSE
-            nRecentSteps := nScalededSteps;
-            nRecentStepAmp := nScalededStepAmp;
-            nRecentStepFreq := nScaledStepFreq;
-            bStepModeparamsSetDone S= TRUE;
-    END_IF
-        fbSetSteps.bExecute := FALSE;
-        fbSetStepFreq.bExecute := FALSE;
-        fbSetStepAmp.bExecute := FALSE;
-    END_IF
-END_IF
-]]></ST>
-      </Implementation>
-    </Method>
-    <Method Name="WriteParameterNC" Id="{a04e1bd8-55ae-495c-bca4-be27f11603b4}">
-      <Declaration><![CDATA[METHOD WriteParameterNC
-VAR_INPUT
-    Execute:BOOL;
-    ParameterNumber : MC_AxisParameter;
-    ParameterValue	: LREAL;
-END_VAR
-
-]]></Declaration>
-      <Implementation>
-        <ST><![CDATA[(*Do not change a moving axis parameter*)
-fbMcWriteParameter(	Axis:=stMotionStage.Axis,
-                    Execute:=(Execute AND stMotionStage.Axis.Status.NotMoving),
-                    ParameterNumber:=ParameterNumber,
-                    Value:=ParameterValue );
-]]></ST>
-      </Implementation>
-    </Method>
-  </POU>
-</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/I_DS402GenericDrive.TcIO b/lcls-twincat-motion/Library/POUs/Motion/DS402/I_DS402GenericDrive.TcIO
deleted file mode 100644
index 3d833b38..00000000
--- a/lcls-twincat-motion/Library/POUs/Motion/DS402/I_DS402GenericDrive.TcIO
+++ /dev/null
@@ -1,137 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<TcPlcObject Version="1.1.0.1">
-  <Itf Name="I_DS402GenericDrive" Id="{49729736-8717-4a13-800f-a53162ccda84}">
-    <Declaration><![CDATA[INTERFACE I_DS402GenericDrive
-]]></Declaration>
-    <Method Name="ExposedParameters" Id="{26e38a32-22d0-42ae-b0c6-5a30c1359c2c}">
-      <Declaration><![CDATA[METHOD ExposedParameters
-VAR_INPUT
-	Enable : BOOL; 
-    tRefreshDelay: TIME;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="Home" Id="{f7b9941e-fd39-45a3-99c7-fc867c3625ba}">
-      <Declaration><![CDATA[METHOD Home
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ModeOperation" Id="{6fb283a3-b1a1-4cbe-9046-2e4da66a2ed6}">
-      <Declaration><![CDATA[METHOD ModeOperation
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="MoveAbsolute" Id="{c32a9e13-f332-4841-9fc3-9751e2083446}">
-      <Declaration><![CDATA[METHOD MoveAbsolute
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="PersistParameters" Id="{2714254e-be9f-442a-9ff0-fec0cfde8ef7}">
-      <Declaration><![CDATA[METHOD PersistParameters
-VAR_INPUT
-	 Enable: BOOL;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="Power" Id="{08548eb9-8f7d-4758-8069-c44e4c6df0ba}">
-      <Declaration><![CDATA[METHOD Power : BOOL
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ReadDriveCodes" Id="{5baeecd3-c54d-4657-a7cc-e3729482e287}">
-      <Declaration><![CDATA[METHOD ReadDriveCodes : BOOL
-]]></Declaration>
-    </Method>
-    <Method Name="Reset" Id="{43fc3192-2758-4d04-872e-08906cd5bf28}">
-      <Declaration><![CDATA[METHOD Reset : BOOL
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="RestoreMotionParams" Id="{b3ff829b-1562-4e27-8092-0b1cc927e533}">
-      <Declaration><![CDATA[METHOD RestoreMotionParams
-VAR_INPUT
-    Enable	: BOOL;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ScaleDriveParams" Id="{8ac3be16-28b8-4175-ae16-72a1e16261fb}">
-      <Declaration><![CDATA[METHOD ScaleDriveParams
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ScaleEncRawValue" Id="{5a3e0ac7-04c3-493b-9cb6-07a60972201e}">
-      <Declaration><![CDATA[METHOD ScaleEncRawValue
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ScaleHomeMotionParams" Id="{2ab106ea-b09a-4819-bf0b-0e6b84eb1cf8}">
-      <Declaration><![CDATA[METHOD ScaleHomeMotionParams
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="ScaleServoOffMotionParams" Id="{b7054ca4-bda3-486f-9d59-a8ac6ece7055}">
-      <Declaration><![CDATA[METHOD ScaleServoOffMotionParams
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="SetEnables" Id="{b93a945e-5ba8-456d-9ebc-47294f36ee3e}">
-      <Declaration><![CDATA[METHOD SetEnables
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="StateMachine" Id="{35531d1d-083e-4e0e-a309-4f04090b40cb}">
-      <Declaration><![CDATA[METHOD StateMachine
-VAR_INPUT
-	Enable : BOOL;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="StepMove" Id="{cdb17cc5-ff2a-49a3-bc83-37fe8e84d0f0}">
-      <Declaration><![CDATA[METHOD  StepMove
-VAR_INPUT
-	Enable : BOOL;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="UpdateDriveMonitoringParams" Id="{8610e279-04bc-4548-942d-f600acfb3079}">
-      <Declaration><![CDATA[METHOD UpdateDriveMonitoringParams
-VAR_INPUT
-    Enable : BOOL;
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="UpdateEpicsStatus" Id="{e22f0ab3-658b-45b7-b368-204fbd2d95fa}">
-      <Declaration><![CDATA[METHOD UpdateEpicsStatus
-VAR_INPUT
-END_VAR
-]]></Declaration>
-    </Method>
-    <Method Name="UpdateServoOffMotionParams" Id="{1a1f77f3-ee6f-4689-a9b6-8149ebbedfec}">
-      <Declaration><![CDATA[METHOD UpdateServoOffMotionParams
-VAR_INPUT
-	Enable : BOOL;
-END_VAR
-
-]]></Declaration>
-    </Method>
-    <Method Name="WriteParameterNC" Id="{3d85a3f6-b752-4960-b363-60741423f54d}">
-      <Declaration><![CDATA[METHOD WriteParameterNC
-VAR_INPUT
-    Execute:BOOL;
-    ParameterNumber : MC_AxisParameter;
-    ParameterValue	: LREAL;
-END_VAR
-]]></Declaration>
-    </Method>
-  </Itf>
-</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU
new file mode 100644
index 00000000..d6c41e15
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Calibrate.TcPOU
@@ -0,0 +1,119 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MCS2Calibrate" Id="{4b86a6e1-43f7-489f-9e64-44b7835fe9ac}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_MCS2Calibrate
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+END_VAR
+
+VAR_OUTPUT
+    bCalibrateMoveBusy        : BOOL;
+    bCalibrateMoveDone        : BOOL;
+    bCalibrateMoveAborted     : BOOL;
+END_VAR
+VAR
+    eStepState	: E_MoveState := E_MoveState.IDLING;
+    {attribute 'hide'}
+    bReset :  BOOL;
+    {attribute 'hide'}
+    bHalt : BOOL;
+    {attribute 'hide'}
+    tonSettlingTime     : TON;
+    {attribute 'hide'}
+    rtCalibrateMove: R_TRIG;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage) THEN
+   RETURN;
+END_IF
+
+rtCalibrateMove(CLK := bEnable);
+
+CASE eStepState OF
+
+    E_MoveState.IDLING:
+        IF rtCalibrateMove.Q THEN
+            bCalibrateMoveBusy := TRUE;
+            bCalibrateMoveDone := FALSE;
+            // Set bits 0-3 for calibration enable
+            stDS402Drive.nDriveControl.3 := TRUE;
+            eStepState := E_MoveState.INIT;
+        END_IF
+
+    E_MoveState.INIT:
+        IF stMotionStage.bError THEN
+            eStepState := E_MoveState.ERROR;
+        ELSIF stDS402Drive.stDriveStatus.Bit12_OpModeSpecific THEN
+            eStepState := E_MoveState.REACHED;
+        ELSE
+            stDS402Drive.nDriveControl.4 := TRUE;
+            tonSettlingTime.IN:=TRUE;
+            eStepState := E_MoveState.IN_PROGRESS;
+        END_IF
+
+    E_MoveState.IN_PROGRESS:
+        IF bHalt OR bReset THEN
+            bCalibrateMoveAborted := TRUE;
+            eStepState := E_MoveState.INTERRUPTED;
+        ELSIF stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
+            stMotionStage.bError := TRUE;
+            eStepState := E_MoveState.ERROR;
+        ELSE
+            IF tonSettlingTime.Q AND stDS402Drive.stDriveStatus.TargetReached
+              AND stDS402Drive.stDriveStatus.Bit12_OpModeSpecific THEN
+                eStepState := E_MoveState.REACHED;
+            END_IF
+        END_IF
+
+    E_MoveState.INTERRUPTED:
+        stDS402Drive.nDriveControl.4 := FALSE;
+        bCalibrateMoveBusy := FALSE;
+        eStepState := E_MoveState.IDLING;
+
+    E_MoveState.REACHED:
+        bCalibrateMoveBusy := FALSE;
+        bCalibrateMoveDone := TRUE;
+        stDS402Drive.nDriveControl.4 := FALSE;
+        eStepState := E_MoveState.DONE;
+    E_MoveState.DONE:
+        IF stMotionStage.bDone THEN
+            eStepState := E_MoveState.IDLING;
+        END_IF
+    E_MoveState.ERROR:
+        bCalibrateMoveBusy := FALSE;
+        IF NOT stMotionStage.bError THEN
+            eStepState := E_MoveState.IDLING;
+        END_IF
+
+END_CASE
+
+tonSettlingTime(PT:=T#80MS);
+bHalt := FALSE;
+bReset := FALSE;]]></ST>
+    </Implementation>
+    <Method Name="Halt" Id="{054cef2f-a2aa-4334-b8f8-3db255f14cab}">
+      <Declaration><![CDATA[METHOD Halt : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bHalt := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Reset" Id="{9e18e8d8-fd2f-4f8f-8fc9-d6ef7ef731d6}">
+      <Declaration><![CDATA[METHOD Reset : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bReset := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU
new file mode 100644
index 00000000..d57f426d
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2DriveManager.TcPOU
@@ -0,0 +1,220 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MCS2DriveManager" Id="{5c10b351-7387-4496-a10a-4c0c0cc328ab}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_MCS2DriveManager
+VAR_INPUT
+    bUserExec : BOOL; // Rising trigger for manual/user request
+    bStartupActive : BOOL;
+END_VAR
+
+VAR_IN_OUT
+    stMotionStage : ST_MotionStage; // Replace type with your actual axis structure
+    stMCS2ChanParameter       : ST_MCS2ChanParameter;
+    stDS402Drive  	: ST_DS402Drive;
+END_VAR
+VAR_OUTPUT
+    bOperational : BOOL;
+    bFault : BOOL;
+    bManualTransition : BOOL;
+    bDS402Manual : BOOL;
+    bCSPModeEnabled : BOOL;
+    bHomeModeEnabled : BOOL;
+    bStepModeEnabled : BOOL;
+    bCalibrationModeEnabled : BOOL;
+END_VAR
+
+VAR
+
+    eStartupState : INT := 0;
+    tonStartupStep : TON;
+    bFaultLast : BOOL := FALSE;
+    ftFaultClear : F_TRIG;
+    eRequestedManualMode : E_DS402OpMode := E_DS402OpMode.CSP;
+    bPendingManualModeSwitch : BOOL := FALSE;
+END_VAR]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+// no hardware ( this just sim mode);
+IF NOT stMCS2ChanParameter.bEnableHardwareOps THEN
+    bCSPModeEnabled := TRUE;
+    RETURN;
+END_IF
+
+bOperational := FALSE;
+bFault := FALSE;
+
+IF     stDS402Drive.stDriveStatus.ReadyToSwitchOn
+   AND stDS402Drive.stDriveStatus.SwitchedOn
+   AND stDS402Drive.stDriveStatus.OperationEnabled
+   AND NOT stDS402Drive.stDriveStatus.Fault
+   AND stDS402Drive.stDriveStatus.VoltageEnabled
+   AND stDS402Drive.stDriveStatus.QuickStopActive
+   AND NOT stDS402Drive.stDriveStatus.SwitchOnDisabled
+THEN
+    bOperational := TRUE;
+ELSIF stDS402Drive.stDriveStatus.Fault THEN
+    bFault := TRUE;
+    bStartupActive := FALSE;
+END_IF
+
+ftFaultClear(CLK := NOT bFaultLast AND bFault);
+IF NOT bFault AND bFaultLast THEN
+    bStartupActive := TRUE;
+    eStartupState := 0;
+END_IF
+bFaultLast := bFault;
+
+// Status flag for manual operation mode (not CSP)
+bDS402Manual := (stDS402Drive.nModeOfOperationDisplay <> E_DS402OpMode.CSP);
+
+// DS402 startup sequence at boot or after fault (until operational)
+IF bStartupActive AND bDS402Manual THEN
+    CASE eStartupState OF
+        0: // CLEAR CONTROL WORD
+            stDS402Drive.nDriveControl := 0;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 1;
+            END_IF
+        1: // RESET FAULT
+            stDS402Drive.nDriveControl := 16#80;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 2;
+            END_IF
+        2: // SHUTDOWN
+            stDS402Drive.nDriveControl := 6;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 3;
+            END_IF
+        3: // SWITCH ON
+            stDS402Drive.nDriveControl := 7;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 4;
+            END_IF
+        4: // ENABLE OPERATION
+            stDS402Drive.nDriveControl := 15;
+            tonStartupStep(IN := TRUE);
+            IF tonStartupStep.Q THEN
+                tonStartupStep(IN := FALSE);
+                eStartupState := 5;
+            END_IF
+        5: // WAIT FOR OPERATIONAL
+            IF bOperational THEN
+                eStartupState := 0;
+                bStartupActive := FALSE;
+            END_IF
+        ELSE
+            eStartupState := 0;
+    END_CASE
+ELSE
+    // regular operation/control logic (only when startup is finished and drive is operational)
+    IF bOperational THEN
+        CASE stDS402Drive.nModeOfOperationDisplay OF
+            E_DS402OpMode.CSP:
+                bCSPModeEnabled := TRUE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.HOME:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := TRUE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.MCS2_OL_STEP_MODE:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := TRUE;
+                bCalibrationModeEnabled := FALSE;
+            E_DS402OpMode.MCS2_CALIBRATION:
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := TRUE;
+            ELSE
+                bCSPModeEnabled := FALSE;
+                bHomeModeEnabled := FALSE;
+                bStepModeEnabled := FALSE;
+                bCalibrationModeEnabled := FALSE;
+        END_CASE
+    ELSE
+        bCSPModeEnabled := FALSE;
+        bHomeModeEnabled := FALSE;
+        bStepModeEnabled := FALSE;
+        bCalibrationModeEnabled := FALSE;
+    END_IF
+
+    // Mode switch/request evaluator
+    IF bUserExec AND (
+           stMCS2ChanParameter.bStepMoveCmd
+        OR stMotionStage.bHomeCmd
+        OR stMCS2ChanParameter.bCalibrationCmd
+        OR stMotionStage.bMoveCmd ) THEN
+
+        // Which mode is requested?
+        IF stMCS2ChanParameter.bStepMoveCmd THEN
+            eRequestedManualMode := E_DS402OpMode.MCS2_OL_STEP_MODE;
+        ELSIF stMotionStage.bHomeCmd THEN
+            eRequestedManualMode := E_DS402OpMode.HOME;
+        ELSIF stMCS2ChanParameter.bCalibrationCmd THEN
+            eRequestedManualMode := E_DS402OpMode.MCS2_CALIBRATION;
+        ELSIF stMotionStage.bMoveCmd THEN
+            eRequestedManualMode := E_DS402OpMode.CSP;
+        END_IF
+
+        // -- Check if we are already in the requested mode --
+        IF stDS402Drive.nModeOfOperationDisplay = eRequestedManualMode THEN
+            IF bOperational THEN
+                bPendingManualModeSwitch := FALSE;
+                bManualTransition := FALSE;
+            ELSE
+                // Already in requested mode but NOT operational: (re-)trigger DS402 startup sequence!
+                bStartupActive := TRUE;
+                eStartupState := 0; // Start DS402 boot up
+                bPendingManualModeSwitch := FALSE;
+                bManualTransition := FALSE;
+            END_IF
+        ELSE
+            IF eRequestedManualMode = E_DS402OpMode.CSP THEN
+                stDS402Drive.nModeOfOperation := E_DS402OpMode.CSP;
+                bPendingManualModeSwitch := FALSE;
+            ELSE
+                stDS402Drive.nDriveControl.4 := FALSE;
+                stDS402Drive.nModeOfOperation := eRequestedManualMode;
+                bPendingManualModeSwitch := TRUE;
+            END_IF
+        END_IF
+    END_IF
+
+    // === Pending manual mode switch ===
+    IF bPendingManualModeSwitch THEN
+        IF stDS402Drive.nModeOfOperationDisplay = eRequestedManualMode THEN
+            IF bOperational THEN
+                // correct mode AND operational; just set the manual transition flag
+                bManualTransition := TRUE;
+                bPendingManualModeSwitch := FALSE;
+            ELSE
+                // correct mode, BUT NOT operational: initiate DS402 startup boot sequence
+                bStartupActive := TRUE;
+                eStartupState := 0;
+                bPendingManualModeSwitch := FALSE;
+            END_IF
+        END_IF
+    END_IF
+
+END_IF
+
+tonStartupStep( PT := T#80MS);]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Home.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Home.TcPOU
new file mode 100644
index 00000000..fbc4a5d6
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2Home.TcPOU
@@ -0,0 +1,135 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MCS2Home" Id="{ce23061a-c26c-43ea-9803-dbb2bfd48aba}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_MCS2Home
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+END_VAR
+
+VAR_OUTPUT
+    bHomeMoveBusy        : BOOL;
+    bHomeMoveDone        : BOOL;
+    bHomeMoveAborted     : BOOL;
+    bReferenced  : BOOL;
+END_VAR
+VAR
+    eStepState           : E_MoveState := E_MoveState.IDLING;
+    {attribute 'hide'}
+    bReset :  BOOL;
+    {attribute 'hide'}
+    bHalt : BOOL;
+    {attribute 'hide'}
+    tonSettlingTime     : TON;
+    {attribute 'hide'}
+    rtHomeMove               : R_TRIG;
+    fbSetPos: MC_SetPosition;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage) THEN
+   RETURN;
+END_IF
+
+rtHomeMove(CLK := bEnable);
+
+CASE eStepState OF
+    E_MoveState.IDLING:
+        IF rtHomeMove.Q THEN
+            bHomeMoveBusy := TRUE;
+            stMotionStage.bDone := FALSE;
+            fbSetPos.Options.ClearPositionLag := TRUE;
+            tonSettlingTime.IN:=FALSE;
+            stDS402Drive.nDriveControl.3 := TRUE;
+            eStepState := E_MoveState.INIT;
+        END_IF
+
+    E_MoveState.INIT:
+        IF stMotionStage.bError THEN
+            eStepState := E_MoveState.ERROR;
+        ELSIF stDS402Drive.stDriveStatus.Bit12_OpModeSpecific THEN // Already referenced
+            bReferenced := TRUE;
+            eStepState := E_MoveState.DONE;
+        ELSE
+            bReferenced := FALSE;
+            stDS402Drive.nDriveControl.4 := TRUE;
+            tonSettlingTime.IN:=TRUE;
+            eStepState := E_MoveState.STARTED;
+        END_IF
+
+    E_MoveState.STARTED:
+        IF NOT stDS402Drive.stDriveStatus.TargetReached
+           AND NOT stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
+            eStepState := E_MoveState.IN_PROGRESS;
+        END_IF
+
+    E_MoveState.IN_PROGRESS:
+        IF bHalt THEN
+            bHomeMoveAborted := TRUE;
+            eStepState := E_MoveState.INTERRUPTED;
+        ELSIF stDS402Drive.stDriveStatus.Bit13_OpModeSpecific
+           OR stDS402Drive.stDriveStatus.WarningCondition THEN
+            stMotionStage.bError := TRUE;
+            eStepState := E_MoveState.ERROR;
+        ELSIF tonSettlingTime.Q AND stDS402Drive.stDriveStatus.TargetReached
+           AND stDS402Drive.stDriveStatus.Bit12_OpModeSpecific
+           AND NOT stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
+            eStepState := E_MoveState.DONE;
+        END_IF
+
+    E_MoveState.INTERRUPTED:
+        stDS402Drive.nDriveControl.4 := FALSE;
+        bHomeMoveBusy := FALSE;
+        eStepState := E_MoveState.IDLING;
+
+    E_MoveState.DONE:
+        bHomeMoveBusy := FALSE;
+        bHomeMoveDone := TRUE;
+        stMotionStage.fPosition := 0.0;
+        IF NOT bReferenced THEN
+            fbSetPos(
+                Axis := stMotionStage.Axis,
+                Execute := TRUE,
+                Position := stMotionStage.fHomePosition
+            );
+        END_IF
+        stDS402Drive.nDriveControl.4 := FALSE;
+        eStepState := E_MoveState.IDLING;
+
+    E_MoveState.ERROR:
+        bHomeMoveDone := FALSE;
+        bHomeMoveBusy := FALSE;
+        IF NOT stMotionStage.bError THEN
+            eStepState := E_MoveState.IDLING;
+        END_IF
+
+END_CASE
+
+tonSettlingTime(PT:=T#80MS);
+bHalt := FALSE;
+bReset := FALSE;]]></ST>
+    </Implementation>
+    <Method Name="Halt" Id="{78d12c46-c454-4a04-8c5c-259167f6d305}">
+      <Declaration><![CDATA[METHOD Halt : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bHalt := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Reset" Id="{5d9bcf8a-a2d5-48c3-b1e1-6c473a08984e}">
+      <Declaration><![CDATA[METHOD Reset : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bReset := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2ReadParams.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2ReadParams.TcPOU
new file mode 100644
index 00000000..dd9fc625
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2ReadParams.TcPOU
@@ -0,0 +1,44 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MCS2ReadParams" Id="{26b1eb2f-7113-4b0e-bfa2-acd56d914490}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_MCS2ReadParams
+VAR_IN_OUT
+    stMCS2Chan1Stage : ST_MotionStage;
+    stMCS2Chan2Stage : ST_MotionStage;
+    stMCS2Chan3Stage : ST_MotionStage;
+END_VAR
+VAR
+    fbReadChan1Params : FB_ReadChanParameters;
+    fbReadChan2Params : FB_ReadChanParameters;
+    fbReadChan3Params : FB_ReadChanParameters;
+    rtExecParamsRead: R_TRIG;
+    tonSDOReadTrigger: TON;
+    bChanReadTrigger: BOOL;
+    nReadTime : TIME  := T#2S;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[rtExecParamsRead(CLK := (stMCS2Chan1Stage.bAxisParamsInit
+                            AND stMCS2Chan2Stage.bAxisParamsInit
+                            AND stMCS2Chan3Stage.bAxisParamsInit
+                ));
+
+IF rtExecParamsRead.Q THEN
+    tonSDOReadTrigger(IN := TRUE);
+END_IF
+
+IF bChanReadTrigger THEN
+    bChanReadTrigger := FALSE;
+    tonSDOReadTrigger(IN := TRUE);
+END_IF
+
+IF tonSDOReadTrigger.Q THEN
+    tonSDOReadTrigger(IN := FALSE);
+    bChanReadTrigger := TRUE;
+END_IF
+
+tonSDOReadTrigger(PT := nReadTime);
+]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2StepMode.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2StepMode.TcPOU
new file mode 100644
index 00000000..62971ad2
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MCS2StepMode.TcPOU
@@ -0,0 +1,114 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MCS2StepMode" Id="{a0cda8fc-6342-4ae1-b3b5-7e5da20f78df}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_MCS2StepMode
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+END_VAR
+
+VAR_OUTPUT
+    bStepMoveBusy        : BOOL;
+    bStepMoveDone        : BOOL;
+    bStepMoveAborted     : BOOL;
+END_VAR
+VAR
+    eStepState           : E_MoveState := E_MoveState.IDLING;
+    bReset :  BOOL;
+    bHalt : BOOL;
+    tonSettlingTime     : TON;
+    {attribute 'hide'}
+    rtStepMove               : R_TRIG;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)  THEN
+   RETURN;
+END_IF
+
+rtStepMove(CLK := bEnable);
+
+CASE eStepState OF
+    E_MoveState.IDLING:
+        IF rtStepMove.Q THEN
+            bStepMoveBusy := TRUE;
+            bStepMoveDone := FALSE;
+            bStepMoveAborted := FALSE;
+            tonSettlingTime.IN:=FALSE;
+            stDS402Drive.nDriveControl.3 := TRUE;
+            eStepState := E_MoveState.INIT;
+        END_IF
+
+    E_MoveState.INIT:
+        IF stMotionStage.bError THEN
+            eStepState := E_MoveState.ERROR;
+        ELSE
+            stDS402Drive.nDriveControl.4 := TRUE;
+            tonSettlingTime.IN:=TRUE;
+            eStepState := E_MoveState.IN_PROGRESS;
+        END_IF
+
+    E_MoveState.IN_PROGRESS:
+        IF bHalt OR bReset THEN
+            bStepMoveAborted := TRUE;
+            eStepState := E_MoveState.INTERRUPTED;
+        ELSIF stDS402Drive.stDriveStatus.Bit13_OpModeSpecific THEN
+            stMotionStage.bError := TRUE;
+            eStepState := E_MoveState.ERROR;
+        ELSE
+            IF tonSettlingTime.Q AND stDS402Drive.stDriveStatus.TargetReached THEN
+                eStepState := E_MoveState.REACHED;
+            END_IF
+        END_IF
+
+    E_MoveState.INTERRUPTED:
+        stDS402Drive.nDriveControl.4 := FALSE;
+        bStepMoveBusy := FALSE;
+        eStepState := E_MoveState.IDLING;
+
+    E_MoveState.REACHED:
+        bStepMoveBusy := FALSE;
+        bStepMoveDone := TRUE;
+        stDS402Drive.nDriveControl.4 := FALSE;
+        eStepState := E_MoveState.DONE;
+
+    E_MoveState.DONE:
+        IF stMotionStage.bDone THEN
+            eStepState := E_MoveState.IDLING;
+        END_IF
+
+    E_MoveState.ERROR:
+        bStepMoveBusy := FALSE;
+        bStepMoveDone := FALSE;
+        IF NOT stMotionStage.bError THEN
+            eStepState := E_MoveState.IDLING;
+        END_IF
+END_CASE
+tonSettlingTime(PT:=T#80MS);
+bHalt := FALSE;
+bReset := FALSE;]]></ST>
+    </Implementation>
+    <Method Name="Halt" Id="{404ed7bc-5145-419a-8c32-b3d5cc2b5db5}">
+      <Declaration><![CDATA[METHOD Halt : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bHalt := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Reset" Id="{0f20cf36-480d-4ea2-8024-db9756e7a2d6}">
+      <Declaration><![CDATA[METHOD Reset : BOOL
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.bReset := TRUE;]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MotionStageMCS2.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MotionStageMCS2.TcPOU
new file mode 100644
index 00000000..2dfc8812
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_MotionStageMCS2.TcPOU
@@ -0,0 +1,1089 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_MotionStageMCS2" Id="{8a1beccb-8ae6-4cef-98b6-1d3261aefb9e}" SpecialFunc="None">
+    <Declaration><![CDATA[(*
+    MCS2 Motion Stage
+    Controls a stage in CSP (NC), STEP_MODE, and Homing via DS402 interface.
+    - Uses overridden Position Holding timer.
+    - power enable mode must be 'DURING_MOTION' for closed-loop (CSP) control.
+    - Set bServo PV to true for closed-loop, clear for open-loop (step mode).
+*)
+{attribute 'call_after_init'}
+FUNCTION_BLOCK FB_MotionStageMCS2
+
+VAR
+    {attribute 'no_copy'}
+    stMotionStage             : REFERENCE TO ST_MotionStage;
+    {attribute 'no_copy'}
+    stMCS2ChanParameter       : REFERENCE TO ST_MCS2ChanParameter;
+    // Hardware I/O
+    stDS402Drive             : ST_DS402Drive;
+
+    fbDriveManager		: FB_MCS2DriveManager;
+    fbReadChanParams	: FB_ReadChanParameters;
+    fbMCS2StepMode 		: FB_MCS2StepMode;
+    fbMCS2home 			: FB_MCS2Home;
+    fbMCS2Calibrate		: FB_MCS2Calibrate;
+    fbStepModeParams 	: FB_WriteStepModeParameters;
+    fbChanParams 		: FB_WriteChanParameters;
+    fbHomingParams 		: FB_WriteHomeParameters;
+    fbCalibrationParams : FB_WriteCalibrationParameters;
+    fbReadDriveCodes : FB_ReadDriveCodes;
+
+    {attribute 'hide'}
+    eModule                 : E_Module;
+    {attribute 'hide'}
+    eHomeMode               : E_EpicsHomeCmd := E_EpicsHomeCmd.AUTOZERO;
+    {attribute 'hide'}
+    eDoneCause              : E_MoveDoneCause := E_MoveDoneCause.Unknown;
+    {attribute 'hide'}
+    fbMcPower               : MC_Power;
+    {attribute 'hide'}
+    fbMcMoveAbsolute        : MC_MoveAbsolute;
+    {attribute 'hide'}
+    fbMcHalt                : MC_Halt;
+    {attribute 'hide'}
+    fbMcReset               : MC_Reset;
+    {attribute 'hide'}
+    fbMcWriteParameter      : MC_WriteParameter;
+    {attribute 'hide'}
+    fbMcReadParams          : MC_ReadParameterSet;
+    {attribute 'hide'}
+    fParameterValue : LREAL;
+    fMeasuredPos: LREAL;
+    fMeasuredVelo: LREAL;
+    fMeasuredAcc: LREAL;
+    bLastHoltTimeOut : BOOL;
+    bRestoreWaitRetry   : BOOL;
+    {attribute 'hide'}
+    bEnPosLag               : BOOL;
+    {attribute 'hide'}
+    bWrongParameter         : BOOL;
+    {attribute 'hide'}
+    bExecMove               : BOOL;
+    {attribute 'hide'}
+    bExecHome               : BOOL;
+    {attribute 'hide'}
+    bNewMoveReq             : BOOL;
+    {attribute 'hide'}
+    bPrepareDisable         : BOOL;
+    {attribute 'hide'}
+    bHalt                   : BOOL;
+    {attribute 'hide'}
+    bLimOverride            : BOOL;
+    {attribute 'hide'}
+    bPositionHold           : BOOL;
+    {attribute 'hide'}
+    bLocalExec              : BOOL;
+    {attribute 'hide'}
+    bSystemInit             : BOOL;
+    {attribute 'hide'}
+    bRestoreLoad             : BOOL;
+    {attribute 'hide'}
+    bExecParamRead          : BOOL;
+
+    {attribute 'hide'}
+    bRecentEnPosLagStatus    : BOOL;
+
+    {attribute 'hide'}
+    bReset                   : BOOL;
+    bFault : BOOL := FALSE;
+    {attribute 'hide'}
+    nCmdDataLocal            : INT;
+    tonHoldTime              : TON;
+    {attribute 'hide'}
+    NCParamsTimer            : TON;
+    {attribute 'hide'}
+    tonRestoreRetry          : TON;
+    {attribute 'hide'}
+    rtExec                   : R_TRIG;
+    {attribute 'hide'}
+    rtMoveCmdShortcut        : R_TRIG;
+    {attribute 'hide'}
+    rtHomeCmdShortcut        : R_TRIG;
+    {attribute 'hide'}
+    rtEnableMode             : R_TRIG;
+    {attribute 'hide'}
+    rtUserExec               : R_TRIG;
+    {attribute 'hide'}
+    rtMoveDone               : R_TRIG;
+    {attribute 'hide'}
+    rtNewMoveReq             : R_TRIG;
+    {attribute 'hide'}
+    ftExec                   : F_TRIG;
+    rtHaltEdge : R_TRIG;
+    {attribute 'hide'}
+    rtStepCmdShortcut		 : R_TRIG;
+    {attribute 'hide'}
+    rtCalibrateCmdShortcut: R_TRIG;
+
+END_VAR
+
+VAR CONSTANT
+    {attribute 'hide'}
+    nChanSoftLimitBase       : WORD := 16#200E;
+    {attribute 'hide'}
+    nChanHomeOffsetBase      : WORD := 16#607C;
+    {attribute 'hide'}
+    nChanHomingSpeedBase     : WORD := 16#6099;
+    {attribute 'hide'}
+    nChanhomingAccBase       : WORD := 16#609A;
+    {attribute 'hide'}
+    nChanErrorCodeBase       : WORD := 16#603F;
+    {attribute 'hide'}
+    nChanFErrWinBase         : WORD := 16#6065;
+    {attribute 'hide'}
+    nChanOfsBase             : WORD := 16#800;      // For SmartAct MCS2 and E-727
+    {attribute 'hide'}
+    nChanEncErrorCode        : WORD := 16#7300;     // Motor encoder defective code (for save/restore)
+    {attribute 'hide'}
+    nChanSoftLimBase         : WORD := 16#200E;
+    {attribute 'hide'}
+    nChanOlStepBase          : WORD := 16#2022;
+    {attribute 'hide'}
+    nChanOlStepAmpBase       : WORD := 16#2023;
+    {attribute 'hide'}
+    nChanOlStepFreqBase      : WORD := 16#2024;
+    {attribute 'hide'}
+    nChanMaxCloseLoopFreqBase: WORD := 16#2001;
+    {attribute 'hide'}
+    nChanPositionerTypeBase  : WORD := 16#2000;
+    {attribute 'hide'}
+    nCHanCalibrationOptBase  : WORD := 16#2004;
+    {attribute 'hide'}
+    nChanSensorPowerModeBase : WORD := 16#2007;
+    {attribute 'hide'}
+    nChanReferencingOptBase  : WORD := 16#200B;
+    {attribute 'hide'}
+    nChanReferenceTypeBase   : WORD := 16#200A;
+    {attribute 'hide'}
+    nChanSafeDirBase         : WORD := 16#200C;
+    {attribute 'hide'}
+    nChanMotorLoadBase       : WORD := 16#200D;
+    {attribute 'hide'}
+    nChannelTypeBase         : WORD := 16#200F;
+    {attribute 'hide'}
+    nChannelStateBase        : WORD := 16#2025;
+    {attribute 'hide'}
+    nChanLogicalScaleOffsetBase  : WORD := 16#2008;
+    {attribute 'hide'}
+    nChanLogicalScaleInvBase     : WORD := 16#2009;
+END_VAR
+
+VAR PERSISTENT
+    nCommandLocal            : INT;
+    bSaved                   : BOOL;
+    fSavedPosition           : LREAL;
+END_VAR]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF( stMotionStage)
+OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+    RETURN;
+END_IF
+
+// when the plc autoboot it appears reading from i/o is not yet after 'call_after_init'
+IF bSystemInit THEN
+    IF  stMotionStage.Axis.NcToPlc.AxisId <> 0 THEN
+        bExecParamRead := TRUE; // trigger NC param read on first scan
+        stMCS2ChanParameter.bStageReady := TRUE;
+        bSystemInit := FALSE;
+        stMCS2ChanParameter.fPosition := stMotionStage.Axis.NcToPlc.ActPos;
+    END_IF
+END_IF
+(* Check for the plc shortcut commands
+   Used for testing or to circumvent motor record issues*)
+rtMoveCmdShortcut(CLK:= stMotionStage.bMoveCmd);
+rtHomeCmdShortcut(CLK:= stMotionStage.bHomeCmd);
+rtStepCmdShortcut(CLK:= stMCS2ChanParameter.bStepMoveCmd);
+rtCalibrateCmdShortcut(CLK:= stMCS2ChanParameter.bCalibrationCmd);
+
+(* Execute on rising edge*)
+IF rtMoveCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
+    stMotionStage.bExecute := TRUE;
+    stMotionStage.nCommand := E_EpicsMotorCmd.MOVE_ABSOLUTE;
+    bEnPosLag := bRecentEnPosLagStatus;
+    fParameterValue := BOOL_TO_LREAL(bRecentEnPosLagStatus);
+
+ELSIF rtStepCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
+    stMotionStage.bExecute := TRUE;
+    stMotionStage.nCommand := E_EpicsMotorCmd.JOG;           // Step mode uses JOG
+    nCommandLocal := E_EpicsMotorCmd.JOG;
+    bEnPosLag := bRecentEnPosLagStatus;
+    fParameterValue := BOOL_TO_LREAL(bRecentEnPosLagStatus);
+
+ELSIF rtCalibrateCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
+    stMotionStage.bExecute := TRUE;
+    stMotionStage.nCommand := E_EpicsMotorCmd.CALIBRATE;     // Calibration mode uses CALIBRATE
+    nCommandLocal := E_EpicsMotorCmd.CALIBRATE;
+    bEnPosLag := bRecentEnPosLagStatus;
+    fParameterValue := BOOL_TO_LREAL(bRecentEnPosLagStatus);
+
+ELSIF rtHomeCmdShortcut.Q AND NOT stMotionStage.bExecute THEN
+    stMotionStage.bExecute := NOT (stMotionStage.nCmdData = E_EpicsHomeCmd.ABSOLUTE_SET)
+                             OR NOT (stMotionStage.nCmdData = E_EpicsHomeCmd.NONE);
+    stMotionStage.nCommand := E_EpicsMotorCmd.HOME;
+    stMotionStage.nCmdData := stMotionStage.nHomingMode;
+    IF stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl.0 THEN
+        bRecentEnPosLagStatus := stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl.0;
+        bEnPosLag := TRUE;
+        fParameterValue := 0;
+    END_IF
+END_IF
+
+(* entry point for local and EPICS main execs *)
+rtUserExec(CLK:= stMotionStage.bExecute);
+(* if a Move/Home Goal is comfirmed and there is no persistant
+   error conditions, then we have a valid move request.*)
+bNewMoveReq S= NOT  stMotionStage.bBusy AND rtUserExec.Q AND NOT  stMotionStage.bError;
+(* this Move request is valid till an error occurs or the currently move is done *)
+bNewMoveReq R= NOT stMotionStage.bExecute OR  stMotionStage.bError;
+bPrepareDisable R= bNewMoveReq;
+
+(* Set the proper command for the request move;
+   if bservo not set, manual step moves will be performed *)
+IF rtUserExec.Q THEN
+    //  if we enter here from state movers
+    stMotionStage.bMoveCmd := TRUE;
+    nCommandLocal := E_EpicsMotorCmd.MOVE_ABSOLUTE;
+    IF stMotionStage.bHomeCmd THEN
+        nCommandLocal := stMotionStage.nCommand;
+        nCmdDataLocal := stMotionStage.nCmdData;
+    ELSIF stMCS2ChanParameter.bStepMoveCmd THEN
+        // For stepmode, directly write the command
+        stMotionStage.nCommand := E_EpicsMotorCmd.JOG;
+        nCommandLocal := E_EpicsMotorCmd.JOG;
+    ELSIF stMCS2ChanParameter.bCalibrationCmd THEN
+        // For calibration, directly write the command
+        stMotionStage.nCommand := E_EpicsMotorCmd.CALIBRATE;
+        nCommandLocal := E_EpicsMotorCmd.CALIBRATE;
+    END_IF
+
+    IF NOT stMotionStage.bHomeCmd AND NOT stMCS2ChanParameter.stChannelState.bIsReferenced THEN
+        // attempting to move an axis without homing first?
+        IF stMotionStage.nHomingMode <> E_EpicsHomeCmd.NONE AND NOT stMotionStage.bHomed THEN
+            stMotionStage.sErrorMessage := 'Axis homing mode set, but homing routine pending';
+        END_IF
+    END_IF
+END_IF
+
+(* Set the drive in the correct operating mode. based on requested move command!
+    NB: bservo must set for close loop motion
+*)
+fbDriveManager(
+    stMotionStage := stMotionStage,
+    stDS402Drive:=stDS402Drive,
+    stMCS2ChanParameter :=stMCS2ChanParameter,
+    bUserExec  := rtUserExec.Q
+);
+
+(* NB: This is the only tested mode of operation so far. also aligned
+    piezo drive position holding feature
+*)
+rtEnableMode(CLK:=( stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION));
+(* Handle auto-enable timing *)
+CASE  stMotionStage.nEnableMode OF
+    (* Not recommended, not tested *)
+    E_StageEnableMode.ALWAYS:
+         stMotionStage.bEnable:=stMotionStage.bSafetyReady;
+    E_StageEnableMode.DURING_MOTION:
+        IF rtEnableMode.Q THEN
+             stMotionStage.bEnable := FALSE;
+        END_IF
+        IF bNewMoveReq THEN
+            (* override ongoing holding.*)
+            bPositionHold := FALSE;
+            tonHoldTime.IN:=FALSE;
+            (*MC block are not compatible with open loop Motion
+                thus we wathc for correct close loop modes to use NC features *)
+             stMotionStage.bEnable :=  stMotionStage.bSafetyReady;
+            bLastHoltTimeOut := FALSE;
+            bHalt:=FALSE;
+        END_IF
+END_CASE
+
+(*  Interlock mainly react on internal limit conditions inthe ove direction.
+    MCS2 drive will go into error state when an internal limit is hit *)
+Interlock();
+
+IF fbDriveManager.bCSPModeEnabled THEN
+    bLocalExec := NOT stMotionStage.bError
+                  AND stMotionStage.bExecute
+                  AND stMotionStage.bAllEnable
+                  AND stMotionStage.bEnableDone
+                  AND stMotionStage.bSafetyReady
+                  AND nCommandLocal = E_EpicsMotorCmd.MOVE_ABSOLUTE;
+ELSIF fbDriveManager.bHomeModeEnabled THEN
+    bLocalExec := NOT stMotionStage.bError
+                  AND stMotionStage.bExecute
+                  AND stMotionStage.bAllEnable
+                  AND fbDriveManager.bOperational
+                  AND stMotionStage.bSafetyReady
+                  AND nCommandLocal = E_EpicsMotorCmd.HOME;
+ELSIF fbDriveManager.bStepModeEnabled THEN
+    bLocalExec := NOT stMotionStage.bError
+                  AND stMotionStage.bExecute
+                  AND stMotionStage.bAllEnable
+                  AND fbDriveManager.bOperational
+                  AND stMotionStage.bSafetyReady
+                  AND nCommandLocal = E_EpicsMotorCmd.JOG;
+ELSIF fbDriveManager.bCalibrationModeEnabled THEN
+    bLocalExec := NOT stMotionStage.bError
+                  AND stMotionStage.bExecute
+                  AND stMotionStage.bAllEnable
+                  AND fbDriveManager.bOperational
+                  AND stMotionStage.bSafetyReady
+                  AND nCommandLocal = E_EpicsMotorCmd.CALIBRATE;
+ELSE
+    bLocalExec := FALSE;
+END_IF
+
+bExecMove:=bLocalExec AND ((nCommandLocal=E_EpicsMotorCmd.MOVE_ABSOLUTE)
+                      OR (nCommandLocal=E_EpicsMotorCmd.JOG)
+                      OR (nCommandLocal=E_EpicsMotorCmd.CALIBRATE));
+bExecHome:=bLocalExec AND NOT bExecMove;
+
+(* When we start, set the busy/done appropriately
+   NB: CLose loop control using NC *)
+rtExec(CLK:=bLocalExec);
+IF rtExec.Q THEN
+    // Release previous interruption if any
+    stDS402Drive.nDriveControl.8 := FALSE;
+    stMotionStage.bDone := FALSE;
+END_IF
+
+(* updated axis status
+   Not needed in manual mode *)
+IF NOT fbDriveManager.bDS402Manual THEN
+     stMotionStage.Axis.ReadStatus();
+END_IF
+
+(* Get a definitive bEnabled reading
+   NB: use only in close loop mode, NC feature needed*)
+CASE  stMotionStage.Axis.Status.MotionState OF
+    (* We are not enabled if there is an issue*)
+    MC_AXISSTATE_UNDEFINED, MC_AXISSTATE_DISABLED, MC_AXISSTATE_ERRORSTOP:
+         stMotionStage.bEnableDone := FALSE;
+    ELSE
+         stMotionStage.bEnableDone := TRUE;
+END_CASE
+//
+IF stMotionStage.bReset THEN
+    // Clear all motion and error flags
+    stMotionStage.bExecute := FALSE;
+    stMotionStage.bError := FALSE;
+    stMotionStage.nErrorId := 0;
+    stMotionStage.sErrorMessage := '';
+    stMotionStage.sCustomErrorMessage := '';
+    fbMCS2StepMode.Reset();
+    fbMCS2home.Reset();
+    bReset := TRUE;
+    IF fbDriveManager.bDS402Manual THEN
+        // Set bit 7 for reset in manual mode
+        stDS402Drive.nDriveControl.7 := TRUE;
+    END_IF
+    stMotionStage.bReset := FALSE;
+END_IF
+
+// EPICS user requested a stop ?
+ftExec(CLK := stMotionStage.bExecute);
+
+bHalt:=ftExec.Q AND stMotionStage.bBusy;
+rtHaltEdge(CLK := bHalt);
+IF rtHaltEdge.Q AND fbDriveManager.bDS402Manual THEN
+    fbMCS2StepMode.Halt();
+    fbMCS2home.Halt();
+    stDS402Drive.nDriveControl.8 := TRUE;
+END_IF
+
+//auto handle position lag monitoring, disable during homing.
+WriteParameterNC(   Execute:=bEnPosLag,
+                    ParameterNumber:=MC_AxisParameter.AxisEnPositionLagMonitoring,
+                    ParameterValue:=fParameterValue);
+fbChanParams (
+    bEnable:=stMotionStage.bAxisParamsInit AND stMCS2ChanParameter.bEnableHardwareOps,
+    stMCS2ChanParameter := stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive,
+    stMotionStage:=stMotionStage,
+);
+
+fbHomingParams (
+    bEnable:=stMotionStage.bAxisParamsInit AND stMCS2ChanParameter.bEnableHardwareOps,
+    stMCS2ChanParameter := stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive,
+    stMotionStage:=stMotionStage,
+);
+
+fbStepModeParams (
+    bEnable:=stMotionStage.bAxisParamsInit AND stMCS2ChanParameter.bEnableHardwareOps,
+    stMCS2ChanParameter := stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive,
+    stMotionStage:=stMotionStage,
+);
+
+fbCalibrationParams (
+    bEnable:=stMotionStage.bAxisParamsInit AND stMCS2ChanParameter.bEnableHardwareOps,
+    stMCS2ChanParameter := stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive,
+    stMotionStage:=stMotionStage,
+);
+(* Requested commands processing *)
+CASE nCommandLocal OF
+    E_EpicsMotorCmd.MOVE_ABSOLUTE:
+        (*NC CSP Move Handling*)
+        Reset();
+        Power();
+        Halt();
+        (*Wait for drive to be in the correct mode after a request was validated *)
+        MoveAbsolute();
+        (*IN CSP mode the drive control word is update via the NC
+          Manual step mode and Homing will override this *)
+        stDS402Drive.nModeOfOperation:= stDS402Drive.nModeOfOperationNC;
+        stDS402Drive.nDriveControl := stDS402Drive.nDriveControlNC;
+    E_EpicsMotorCmd.HOME:
+        (*DS402 Manual Homing*)
+        fbMCS2home(
+            bEnable:=bExecHome AND fbChanParams.bChanParamsSet AND fbHomingParams.bHomingParamsSet,
+            stDS402Drive:=stDS402Drive,
+            stMotionStage:=stMotionStage,
+        );
+    E_EpicsMotorCmd.JOG:
+        (* Set drive to correct operating mode, ensure motion params are correct
+          following a validated open loop mode request.
+          NB: bservo must be off. a PV is provided for that
+        *)
+        fbMCS2StepMode(
+            bEnable:=bExecMove AND fbStepModeParams.bStepModeParamsSet,
+            stDS402Drive:=stDS402Drive,
+            stMotionStage:=stMotionStage,
+        );
+    E_EpicsMotorCmd.CALIBRATE:
+        fbMCS2Calibrate(
+            bEnable:=bExecMove AND fbChanParams.bChanParamsSet AND fbCalibrationParams.bCalibrationsParamsSet,
+            stDS402Drive:=stDS402Drive,
+            stMotionStage:=stMotionStage,
+        );
+END_CASE
+
+stMotionStage.bBusy := fbMcMoveAbsolute.Busy
+                        OR stMCS2ChanParameter.stChannelState.bReferencing
+                        OR fbMCS2StepMode.bStepMoveBusy
+                        OR stMCS2ChanParameter.stChannelState.bCalibrating;
+(* Check done moving via user stop, limit hit, Target Position reached, or from homing.*)
+PostMoveChecks();
+
+(* update encoder value and calibrated position*)
+ScaleEncRawValue();
+
+(*Drive parameters*)
+ExposedParameters(Enable:=stMCS2ChanParameter.bStageReady, tRefreshDelay:=T#2S);
+
+(*Read MCS2 Channel Parameters*)
+fbReadChanParams(
+    bEnable:= stMCS2ChanParameter.bEnableHardwareOps,
+    stMotionStage:=stMotionStage,
+    stMCS2ChanParameter:=stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive,
+);
+
+(* Save and restore as long as not an absolute encoder*)
+PersistParameters( Enable:= stMCS2ChanParameter.bStageReady);
+
+(*Restore encoder value at initialization*)
+RestoreMotionParams(Enable:= stMCS2ChanParameter.bStageReady);
+
+(*EPICS Motor record Update*)
+UpdateEpicsStatus();
+(*
+    Error from functions and Nc. The error will send to EPICS interface based on predifined
+    priority: axis, power, backlash, absoluteMove, etc...
+*)
+IF fbMcPower.Error AND fbMcPower.Active THEN
+     stMotionStage.bError:=fbMcPower.Error;
+     stMotionStage.nErrorId:=fbMcPower.ErrorID;
+ELSIF fbMcMoveAbsolute.Error THEN
+     stMotionStage.bError:=fbMcMoveAbsolute.Error;
+     stMotionStage.nErrorId:=fbMcMoveAbsolute.ErrorID;
+ELSIF fbMcHalt.Error AND fbMcHalt.Active THEN
+     stMotionStage.bError:=fbMcHalt.Error;
+     stMotionStage.nErrorId:=fbMcHalt.ErrorID;
+ELSIF fbMcReset.Error  THEN
+     stMotionStage.bError:=fbMcReset.Error;
+     stMotionStage.nErrorId:=fbMcReset.ErrorID;
+ELSIF fbMcWriteParameter.Error THEN
+     stMotionStage.bError:=fbMcWriteParameter.Error;
+     stMotionStage.nErrorId:=fbMcWriteParameter.ErrorID;
+ELSIF fbMcReadParams.Error AND NOT  stMotionStage.bBusy THEN
+     stMotionStage.bError:=fbMcReadParams.Error;
+     stMotionStage.nErrorId:=fbMcReadParams.ErrorID;
+ELSE
+    IF  stMotionStage.bBusy THEN
+         stMotionStage.sErrorMessage := '';
+         stMotionStage.sCustomErrorMessage := '';
+    END_IF
+END_IF;
+
+(*Double function, prioritize NC error otherwise read drive channel error code*)
+fbReadDriveCodes(
+    bEnable:= stMCS2ChanParameter.bEnableHardwareOps,
+    stMotionStage:=stMotionStage,
+    stMCS2ChanParameter:=stMCS2ChanParameter,
+    stDS402Drive:=stDS402Drive
+);
+
+IF  stMotionStage.sCustomErrorMessage <> ''
+    AND  stMotionStage.sErrorMessage = '' THEN
+     stMotionStage.sErrorMessage :=  stMotionStage.sCustomErrorMessage;
+END_IF
+
+(*Clear motion flag when error occurs*)
+IF  stMotionStage.bError  THEN
+    stMotionStage.bExecute := FALSE;
+    stMotionStage.bBusy := FALSE;
+    stMotionStage.bDone := FALSE;
+    stMotionStage.bEnable := FALSE;
+END_IF
+
+(* We've got the rising edge clear this flags.*)
+stMotionStage.bMoveCmd := FALSE;
+stMotionStage.bHomeCmd := FALSE;
+stMCS2ChanParameter.bStepMoveCmd := FALSE;
+stMCS2ChanParameter.bCalibrationCmd := FALSE;
+]]></ST>
+    </Implementation>
+    <Method Name="ExposedParameters" Id="{bfdcb735-2f35-43ca-9b4a-eca418143c19}">
+      <Declaration><![CDATA[METHOD PUBLIC  ExposedParameters
+VAR_INPUT
+    Enable : BOOL;
+    tRefreshDelay: TIME;
+END_VAR
+
+VAR_INST
+    {attribute 'hide'}
+    bParamReadTimer     : BOOL := TRUE;
+END_VAR
+
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[// Periodic update of NC parameters
+NCParamsTimer( IN:=Enable AND NOT bExecParamRead, PT:=tRefreshDelay);
+bExecParamRead S= NCParamsTimer.Q;
+fbMcReadParams(
+    Parameter:= stMotionStage.stAxisParameters,
+    Axis:= stMotionStage.Axis,
+    Execute:= bExecParamRead );
+
+// Copy axis parameters that we want to expose to the EPICS layer.
+ stMotionStage.stAxisParametersExposed.bCtrlEnablePosDiffControl     :=  stMotionStage.stAxisParameters.bCtrlEnablePosDiffControl;
+ stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMaxControl   :=  stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl;
+ stMotionStage.stAxisParametersExposed.bEncEnableSoftEndMinControl   :=  stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl;
+ stMotionStage.stAxisParametersExposed.fAccelerationMax              :=  stMotionStage.stAxisParameters.fAccelerationMax;
+ stMotionStage.stAxisParametersExposed.fCtrlPosDiffMax               :=  stMotionStage.stAxisParameters.fCtrlPosDiffMax;
+ stMotionStage.stAxisParametersExposed.fCtrlPosDiffMaxTime           :=  stMotionStage.stAxisParameters.fCtrlPosDiffMaxTime;
+ stMotionStage.stAxisParametersExposed.fDecelerationMax              :=  stMotionStage.stAxisParameters.fDecelerationMax;
+ stMotionStage.stAxisParametersExposed.fEncSoftEndMax                :=  stMotionStage.stAxisParameters.fEncSoftEndMax;
+ stMotionStage.stAxisParametersExposed.fEncSoftEndMin                :=  stMotionStage.stAxisParameters.fEncSoftEndMin;
+ stMotionStage.stAxisParametersExposed.fVeloMaximum                  :=  stMotionStage.stAxisParameters.fVeloMaximum;
+ stMotionStage.stAxisParametersExposed.fEncOffset                       :=  stMotionStage.stAxisParameters.fEncOffset;
+ stMotionStage.stAxisParametersExposed.fEncScaleFactorInternal       :=  stMotionStage.stAxisParameters.fEncScaleFactorInternal;
+
+bExecParamRead R= fbMcReadParams.Done OR fbMcReadParams.Error;
+ stMotionStage.bAxisParamsInit S= fbMcReadParams.Done;]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="FB_Init" Id="{a8134580-303c-4b2f-8942-be52670189d3}">
+      <Declaration><![CDATA[//FB_Init is always available implicitly and it is used primarily for initialization.
+//The return value is not evaluated. For a specific influence, you can also declare the
+//methods explicitly and provide additional code there with the standard initialization
+//code. You can evaluate the return value.
+METHOD FB_Init: BOOL
+VAR_INPUT
+    bInitRetains: BOOL; // TRUE: the retain variables are initialized (reset warm / reset cold)
+    bInCopyCode: BOOL;  // TRUE: the instance will be copied to the copy code afterward (online change)
+    stMotionStage	: REFERENCE TO ST_MotionStage;
+    stMCS2ChanParameter : REFERENCE TO ST_MCS2ChanParameter;
+    (*Mandatory must be unique for each MCS2 axis *)
+    eModule : E_Module;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[THIS^.stMotionStage REF= stMotionStage;
+THIS^.stMCS2ChanParameter REF= stMCS2ChanParameter;
+THIS^.eModule := eModule;
+
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Halt" Id="{6287ad75-ad53-4d78-9e75-75537e8950c5}">
+      <Declaration><![CDATA[METHOD Halt
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[fbMcHalt(
+    Axis :=  stMotionStage.Axis,
+    Execute := bHalt,
+    Deceleration :=  stMotionStage.fDeceleration,
+    BufferMode := MC_BufferMode.MC_Aborting,
+);
+IF fbMcHalt.Done THEN
+    bHalt := FALSE;
+END_IF
+
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Interlock" Id="{b0516fc0-3ac5-4a37-a4b7-b21194f9b805}">
+      <Declaration><![CDATA[METHOD PUBLIC Interlock
+
+VAR_INST
+    {attribute 'hide'}
+    bPositiveDirection  : BOOL;
+    {attribute 'hide'}
+    bNegativeDirection  : BOOL;
+    {attribute 'hide'}
+    ftForwardEnabled    : F_TRIG;
+
+    {attribute 'hide'}
+    ftBackwardEnabled   : F_TRIG;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[// Positive or Negative direction
+bPositiveDirection:= stMotionStage.bBusy AND  stMotionStage.fPosDiff > 0;
+bNegativeDirection:= stMotionStage.bBusy AND  stMotionStage.fPosDiff < 0;
+
+// For SmartAct MCS2 these limit are range and enstop limits. however a single bit in the status is used
+// bLimOverride  : act as an override after limit was hit to allow reverse movement
+ stMotionStage.bLimitForwardEnable :=  bLimOverride OR NOT( bPositiveDirection AND stDS402Drive.stDriveStatus.InternalLimitActive );
+ stMotionStage.bLimitBackwardEnable := bLimOverride OR NOT( bNegativeDirection AND stDS402Drive.stDriveStatus.InternalLimitActive );
+
+// use falling trigger to avoid spaming  sErrorMessage
+ftForwardEnabled(CLK:= stMotionStage.bLimitForwardEnable);
+ftBackwardEnabled(CLK:= stMotionStage.bLimitBackwardEnable);
+
+IF  NOT fbMCS2home.bHomeMoveBusy AND ftForwardEnabled.Q THEN
+    // Not an error, just a warning
+     stMotionStage.sCustomErrorMessage:='Cannot move past positive limit.';
+    IF NOT fbDriveManager.bStepModeEnabled THEN
+        bLimOverride := TRUE;
+        bHalt := TRUE;
+    END_IF
+END_IF
+
+IF NOT fbMCS2home.bHomeMoveBusy AND ftBackwardEnabled.Q THEN
+    // Not an error, just a warning
+     stMotionStage.sCustomErrorMessage:='Cannot move past Negative limit.';
+    IF NOT fbDriveManager.bStepModeEnabled THEN
+        bLimOverride := TRUE;
+        bHalt := TRUE;
+    END_IF
+END_IF
+
+IF NOT  stMotionStage.bError AND  stMotionStage.bExecute AND NOT  stMotionStage.bUserEnable THEN
+     stMotionStage.bError := TRUE;
+     stMotionStage.nErrorId := 1;
+     stMotionStage.sCustomErrorMessage := 'Move requested, but user enable is disabled!';
+END_IF
+
+// Update all enable booleans
+SetEnables();]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="MoveAbsolute" Id="{c9986ba7-a025-4f1a-8312-93dc4112a24c}">
+      <Declaration><![CDATA[METHOD PUBLIC  MoveAbsolute
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[fbMcMoveAbsolute(
+    Axis :=  stMotionStage.Axis,
+    Execute := bExecMove AND fbMcPower.Status AND fbDriveManager.bCSPModeEnabled,
+    Position :=  stMotionStage.fPosition,
+    Velocity :=  stMotionStage.fVelocity,
+    Acceleration :=  stMotionStage.fAcceleration,
+    Deceleration :=  stMotionStage.fDeceleration,
+    BufferMode := MC_BufferMode.MC_Aborting,
+);
+
+IF fbMcMoveAbsolute.Done (*OR fbMcReset.Done*) THEN
+    (*Hold the position in close loop before dropping power, a positional drift will occur then*)
+    IF ( stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION ) THEN
+        bPositionHold := TRUE;
+        tonHoldTime.IN:=TRUE;
+    END_IF
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="PersistParameters" Id="{827e19b0-eb90-45f1-a0f0-e397f2f834ae}">
+      <Declaration><![CDATA[METHOD PersistParameters
+VAR_INPUT
+    Enable  : BOOL;
+END_VAR
+
+VAR_INST
+    {attribute 'hide'}
+    bEncError           : BOOL;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[// Save Encoder position
+// needs some pre-conditions to use persistent storage
+IF Enable THEN
+    // Check ST_MotionStage for an encoder error (range 0x44nn)
+    bEncError:=  stMotionStage.Axis.Status.Error AND  (  stMotionStage.Axis.Status.ErrorID  >= 16#4400  AND  stMotionStage.Axis.Status.ErrorID <= 16#44FF );
+    // Do not save if we're currently loading or if there is an encoder error
+    IF NOT bRestoreLoad AND NOT bEncError AND NOT bRestoreWaitRetry THEN
+        fSavedPosition :=  stMotionStage.Axis.NcToPlc.ActPos;
+        // This persistent variable lets us check if anything was saved
+        // It will be TRUE at startup if we have saved values
+        bSaved := TRUE;
+    END_IF
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="PostInit" Id="{7af64fe1-72e2-4a67-9fd4-3933db27c0b4}">
+      <Declaration><![CDATA[{attribute 'call_after_init'}
+(*  eModule: Mandatory must be unique for each MCS2 axis
+    clears startup errors or warnings. the process is completed at run time
+    SDO update of Drive and open Loop parameter Indexes based on module number
+*)
+METHOD PostInit
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[(* SDO update of Drive and open Loop parameter Indexes based on module number *)
+stMCS2ChanParameter.nChanErrorCodeIdx        	:= nChanErrorCodeBase         	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanFErrWinIdx          	:= nChanFErrWinBase           	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanSoftLimitIdx        	:= nChanSoftLimitBase     		+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanHomeOffsIdx         	:= nChanHomeOffsetBase        	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanHomeVeloIdx         	:= nChanHomingSpeedBase       	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanhomeAccIdx          	:= nChanhomingAccBase         	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanStepIdx             	:= nChanOlStepBase            	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanStepFreqIdx         	:= nChanOlStepFreqBase        	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanStepAmpIdx          	:= nChanOlStepAmpBase         	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanMaxCloseLoopFreqIdx    := nChanMaxCloseLoopFreqBase  	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanPositionerTypeIdx      := nChanPositionerTypeBase    	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanCalibrationOptIdx      := nCHanCalibrationOptBase    	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanSensorPowerModeIdx     := nChanSensorPowerModeBase   	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanReferencingOptIdx      := nChanReferencingOptBase    	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanReferenceTypeIdx       := nChanReferenceTypeBase     	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanSafeDirIdx             := nChanSafeDirBase           	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanMotorLoadIdx           := nChanMotorLoadBase         	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChannelTypeIdx          	:= nChannelTypeBase       		+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanStateIdx         		:= nChannelStateBase      		+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanLogicalScaleOfsIdx   	:= nChanLogicalScaleOffsetBase	+ ((eModule-1) * nChanOfsBase);
+stMCS2ChanParameter.nChanLogicalScaleInvIdx     := nChanLogicalScaleInvBase   	+ ((eModule-1) * nChanOfsBase);
+bSystemInit := TRUE;
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="PostMoveChecks" Id="{20c015e9-bf00-4308-96ac-d493b364e91d}">
+      <Declaration><![CDATA[METHOD PRIVATE PostMoveChecks : BOOL
+VAR_INST
+    rtAbortedHome        : R_TRIG;
+    rtAbortedStepMove    : R_TRIG;
+    rtAbortedCalibration : R_TRIG;
+    rtResetDone          : R_TRIG;
+    rtHaltDone           : R_TRIG;
+    rtMoveDone           : R_TRIG;
+    rtHomeDone           : R_TRIG;
+    rtStepMoveDone       : R_TRIG;
+    rtCalibrationDone    : R_TRIG;
+    ftEnableFallEdge     : F_TRIG;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[rtAbortedHome(CLK := fbMCS2home.bHomeMoveAborted);
+rtAbortedStepMove(CLK := fbMCS2StepMode.bStepMoveAborted);
+rtAbortedCalibration(CLK := fbMCS2Calibrate.bCalibrateMoveAborted);
+
+rtResetDone(CLK := fbMcReset.Done);
+rtHaltDone(CLK := fbMcHalt.Done);
+
+rtMoveDone(CLK := fbMcMoveAbsolute.Done);
+rtHomeDone(CLK := fbMCS2home.bHomeMoveDone);
+rtStepMoveDone(CLK := fbMCS2StepMode.bStepMoveDone);
+rtCalibrationDone(CLK := fbMCS2Calibrate.bCalibrateMoveDone);
+
+IF rtAbortedHome.Q OR rtAbortedStepMove.Q OR rtAbortedCalibration.Q
+   OR rtResetDone.Q OR rtHaltDone.Q
+   OR rtMoveDone.Q OR rtHomeDone.Q OR rtStepMoveDone.Q OR rtCalibrationDone.Q THEN
+
+    IF NOT stMotionStage.bDone THEN
+        stMotionStage.bExecute := FALSE;;
+        stMotionStage.bHomed := fbMCS2home.bHomeMoveDone;
+        IF rtMoveDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.MoveDone;
+        ELSIF rtStepMoveDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.StepMoveDone;
+        ELSIF rtHomeDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.HomeDone;
+        ELSIF rtCalibrationDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.CalibrationDone;
+        ELSIF rtResetDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.ResetDone;
+        ELSIF rtHaltDone.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.HaltDone;
+        ELSIF rtAbortedHome.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.AbortedHome;
+        ELSIF rtAbortedStepMove.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.AbortedStepMove;
+        ELSIF rtAbortedCalibration.Q THEN
+            stMotionStage.bDone := TRUE;
+            eDoneCause := E_MoveDoneCause.AbortedCalibration;
+        END_IF
+        IF fbDriveManager.bDS402Manual THEN
+            bEnPosLag := FALSE;
+        END_IF
+        bLimOverride := FALSE;
+        IF (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION) THEN
+            bPositionHold := TRUE;
+            tonHoldTime.IN := TRUE;
+        END_IF
+        // get stable current position storage
+        stMCS2ChanParameter.fPosition := stMotionStage.fPosition;
+    END_IF
+END_IF
+
+tonHoldTime(PT := UDINT_TO_TIME(stMCS2ChanParameter.nHoldTime));
+IF tonHoldTime.Q THEN
+    bPositionHold := FALSE;
+    tonHoldTime.IN := FALSE;
+    bLastHoltTimeOut := TRUE;
+END_IF
+
+(* updated axis status
+   Not needed in manual mode *)
+IF NOT fbDriveManager.bDS402Manual THEN
+     stMotionStage.Axis.ReadStatus();
+END_IF
+
+bPrepareDisable S= (stMotionStage.nEnableMode = E_StageEnableMode.DURING_MOTION) AND ftExec.Q;
+
+IF bPrepareDisable AND fbDriveManager.bCSPModeEnabled THEN
+    IF stMotionStage.Axis.Status.StandStill THEN
+        IF NOT bPositionHold THEN
+            bPrepareDisable := FALSE;
+            stMotionStage.bEnable := FALSE; // TwinCAT NC disables MC_Power (CSP)
+        END_IF
+    END_IF
+ELSIF bPrepareDisable AND fbDriveManager.bDS402Manual THEN
+    IF fbDriveManager.bOperational THEN
+        IF NOT bPositionHold THEN
+            bPrepareDisable := FALSE;
+            stMotionStage.bEnable := FALSE; // Manual disables via drive control word
+        END_IF
+    END_IF
+END_IF
+
+ftEnableFallEdge(CLK := stMotionStage.bEnable);
+IF ftEnableFallEdge.Q THEN
+    IF fbDriveManager.bDS402Manual THEN
+        stDS402Drive.nDriveControl := 6; // Manual mode power off
+    END_IF
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Power" Id="{f641a677-a472-4a34-a0e0-f26f3662273e}">
+      <Declaration><![CDATA[METHOD Power
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[fbMcPower(
+    Axis :=  stMotionStage.Axis,
+    Enable :=  stMotionStage.bAllEnable,
+    Enable_Positive :=  stMotionStage.bAllForwardEnable AND stMCS2ChanParameter.bOverrideDirEnable,
+    Enable_Negative :=  stMotionStage.bAllBackwardEnable AND stMCS2ChanParameter.bOverrideDirEnable,
+    Override := 100.0,
+    BufferMode := MC_BufferMode.MC_Aborting
+);]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="Reset" Id="{643b4736-0a43-4a18-8f6b-8a2439ed5e6f}">
+      <Declaration><![CDATA[METHOD Reset
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[fbMcReset(
+    Axis :=  stMotionStage.Axis,
+    Execute := bReset
+);
+IF fbMcReset.Done THEN
+    bReset := FALSE;
+END_IF
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="RestoreMotionParams" Id="{8335d215-fac8-43bf-97f2-e08acc164700}">
+      <Declaration><![CDATA[METHOD RestoreMotionParams
+VAR_INPUT
+    Enable  : BOOL;
+END_VAR
+
+VAR_INST
+    fbSetPos            : MC_SetPosition;
+    {attribute 'hide'}
+    bLoad               : BOOL;
+    {attribute 'hide'}
+    bRestoreInit        : BOOL;
+    {attribute 'hide'}
+    nMaxRetries         : UINT := 10;
+    {attribute 'hide'}
+    nCurrTries          : UINT := 0;
+    {attribute 'hide'}
+    nLatchError         : UDINT;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[IF Enable THEN
+    // Trigger a load if anything was saved at all
+    IF NOT bRestoreInit THEN
+        bRestoreInit := TRUE;
+        bRestoreLoad S= bSaved;
+        fbSetPos.Options.ClearPositionLag := TRUE;
+    END_IF
+
+    // Set our position if bRestoreLoad is true
+    fbSetPos( Axis:= stMotionStage.Axis, Execute:=bRestoreLoad, Position:=fSavedPosition);
+
+    IF fbSetPos.Error THEN
+        // Keep the error latched, it can disappear if Execute is set to FALSE
+        nLatchError := fbSetPos.ErrorID;
+        nCurrTries := nCurrTries + 1;
+        IF nCurrTries >= nMaxRetries THEN
+        // Alert the user that something has gone wrong
+         stMotionStage.bError := TRUE;
+         stMotionStage.nErrorId := nLatchError;
+         stMotionStage.sCustomErrorMessage := 'Error loading previously saved position.';
+        ELSE
+            // Reset the FB for the next retry
+            fbSetPos( Axis:= stMotionStage.Axis, Execute:=bRestoreLoad, Position:=fSavedPosition);
+            // Try again
+            bRestoreWaitRetry := TRUE;
+        END_IF
+    ELSIF fbSetPos.Done THEN
+         stMotionStage.fPosition := fSavedPosition;
+    END_IF
+
+    // Only load once, at startup
+    bRestoreLoad R= fbSetPos.Done OR fbSetPos.Error;
+
+    tonRestoreRetry( IN := bRestoreWaitRetry, PT := T#100MS);
+    bRestoreLoad S= tonRestoreRetry.Q;
+    bRestoreWaitRetry R= tonRestoreRetry.Q;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="ScaleEncRawValue" Id="{769f98a6-388a-461f-854a-93b1758d578f}">
+      <Declaration><![CDATA[METHOD ScaleEncRawValue
+VAR_INPUT
+  //  bEnable: BOOL;
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[(* SmartACT: The encoder count from the Piezo drive is not a raw count from the embeded actuator device
+   we get a calibrated nm value. thus the use of abs() since this value is signed.
+   PI: to be discovered *)
+IF  stMotionStage.nRawEncoderDINT <> 0 THEN
+     stMotionStage.nEncoderCount:=DINT_TO_UDINT(ABS( stMotionStage.nRawEncoderDINT));
+ELSE
+     stMotionStage.nEncoderCount:=0;
+END_IF
+
+// calibrated encoder readback
+IF NOT fbDriveManager.bStepModeEnabled THEN
+    // Close loop NC
+    fMeasuredPos:= stMotionStage.Axis.NcToPlc.ActPos;
+    fMeasuredVelo := stMotionStage.Axis.NcToPlc.ActVelo;
+    fMeasuredAcc :=  stMotionStage.Axis.NcToPlc.ActAcc;
+     stMotionStage.fPosDiff:= stMotionStage.Axis.NcToPlc.PosDiff;
+ELSE
+    fMeasuredPos:=DINT_TO_REAL( stMotionStage.nRawEncoderDINT) * MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor);
+     stMotionStage.fPosDiff:= stMotionStage.fPosition - fMeasuredPos;
+    // NB: Not actual in open loop
+    fMeasuredVelo := stMotionStage.fVelocity;
+    fMeasuredAcc :=  stMotionStage.fAcceleration;
+END_IF
+
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="SetEnables" Id="{28f7e94e-bad4-40ca-b34a-f8da453343cc}">
+      <Declaration><![CDATA[METHOD SetEnables
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[ stMotionStage.bAllForwardEnable:= stMotionStage.bLimitForwardEnable
+                                    AND ( stMotionStage.bGantryForwardEnable OR NOT  stMotionStage.bGantryAxis)
+                                    AND  stMotionStage.stEPSForwardEnable.bEPS_OK;
+                                    //AND  stMCS2ChanParameter.bOverrideDirEnable;
+ stMotionStage.bAllBackwardEnable:= stMotionStage.bLimitBackwardEnable
+                                    AND ( stMotionStage.bGantryBackwardEnable OR NOT  stMotionStage.bGantryAxis)
+                                    AND  stMotionStage.stEPSBackwardEnable.bEPS_OK;
+                                    //AND  stMCS2ChanParameter.bOverrideDirEnable;
+
+ stMotionStage.bAllEnable:= stMotionStage.bEnable
+                            AND  stMotionStage.bHardwareEnable
+                            AND  stMotionStage.stEPSPowerEnable.bEPS_OK;
+ stMotionStage.bAllEnable R= NOT  stMotionStage.bUserEnable;]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="UpdateEpicsStatus" Id="{dc90c8f8-9d88-4ab9-9dc0-f90cd21b81fc}">
+      <Declaration><![CDATA[METHOD UpdateEpicsStatus
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[//////////////////////////////////////////////////////////////////
+// Transfer NC parameter status to EPICS
+//////////////////////////////////////////////////////////////////
+ stMotionStage.nMotionAxisID :=  stMotionStage.Axis.NcToPlc.AxisId;
+ stMotionStage.stAxisStatus.bEnable:= stMotionStage.bAllEnable;
+ stMotionStage.stAxisStatus.bEnabled:= stMotionStage.bEnableDone; // account for PowerEnable from power block
+ stMotionStage.stAxisStatus.bError:= stMotionStage.bError;
+ stMotionStage.stAxisStatus.bHomeSensor:= stMotionStage.bHome;
+ stMotionStage.stAxisStatus.bLimitBwd:= stMotionStage.bAllBackwardEnable;
+ stMotionStage.stAxisStatus.bLimitFwd:= stMotionStage.bAllForwardEnable;
+ stMotionStage.stAxisStatus.bReset:= stMotionStage.bReset;
+ stMotionStage.stAxisStatus.fAcceleration:=fMeasuredAcc;
+ stMotionStage.stAxisStatus.fActDiff:= stMotionStage.fPosDiff;
+ stMotionStage.stAxisStatus.fActPosition:=fMeasuredPos;
+ stMotionStage.stAxisStatus.fActVelocity:=fMeasuredVelo;
+ stMotionStage.stAxisStatus.fDeceleration:=fMeasuredAcc;
+ stMotionStage.stAxisStatus.fOverride:=fbMcPower.Override;
+ stMotionStage.stAxisStatus.fPosition:= stMotionStage.fPosition;
+ stMotionStage.stAxisStatus.fVelocity:= stMotionStage.fVelocity;
+ stMotionStage.stAxisStatus.nCmdData:=INT_TO_UINT( stMotionStage.nCmdData);  //Or nCmdDataLocal
+ stMotionStage.stAxisStatus.nCommand:=INT_TO_UINT( stMotionStage.nCommand);  //Or nCommandLocal
+ stMotionStage.stAxisStatus.nErrorId:= stMotionStage.nErrorId;
+ stMotionStage.stAxisStatus.bBusy:= stMotionStage.bBusy;
+ stMotionStage.stAxisStatus.bHomed:= stMotionStage.bHomed;
+ stMotionStage.stAxisStatus.bExecute:=bLocalExec;
+ stMotionStage.stAxisStatus.nCommand:=3; // If this is not 3, the IOC stops updating positions during homing
+
+
+]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="WriteParameterNC" Id="{a04e1bd8-55ae-495c-bca4-be27f11603b4}">
+      <Declaration><![CDATA[METHOD WriteParameterNC
+VAR_INPUT
+    Execute:BOOL;
+    ParameterNumber : MC_AxisParameter;
+    ParameterValue  : LREAL;
+END_VAR
+
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[(*Do not change a moving axis parameter*)
+fbMcWriteParameter(
+    Axis:=stMotionStage.Axis,
+    Execute:=(Execute AND stMotionStage.Axis.Status.NotMoving),
+    ParameterNumber:=ParameterNumber,
+    Value:=ParameterValue
+);
+]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadChanParameters.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadChanParameters.TcPOU
new file mode 100644
index 00000000..caf0f0dc
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadChanParameters.TcPOU
@@ -0,0 +1,250 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_ReadChanParameters" Id="{08ee74cb-7e7d-468d-8ff5-637be4dc44de}" SpecialFunc="None">
+    <Declaration><![CDATA[FUNCTION_BLOCK FB_ReadChanParameters
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+
+VAR_IN_OUT
+    stMotionStage : ST_MotionStage;
+    stMCS2ChanParameter : ST_MCS2ChanParameter;
+    stDS402Drive  	: ST_DS402Drive;
+END_VAR
+
+VAR_OUTPUT
+    bAllReadsComplete : BOOL;
+END_VAR
+
+VAR
+    bChanReadTrigger : BOOL;
+    tonSDOReadTrigger          : TON ;
+    fbMaxCloseLoopFreqRead     : FB_EcCoESdoRead;
+    fbPositionerTypeRead       : FB_EcCoESdoRead;
+    fbCalibrationOptRead       : FB_EcCoESdoRead;
+    fbSensorPowerModeRead      : FB_EcCoESdoRead;
+    fbReferencingOptRead       : FB_EcCoESdoRead;
+    fbReferenceTypeRead        : FB_EcCoESdoRead;
+    fbSafeDirRead              : FB_EcCoESdoRead;
+    fbMotorLoadRead            : FB_EcCoESdoRead;
+    fbChannelTypeRead          : FB_EcCoESdoRead;
+    fbChannelStateRead         : FB_EcCoESdoRead;
+    fbLogicalScaleOffsetRead   : FB_EcCoESdoRead;
+    fbLogicalScaleInvRead      : FB_EcCoESdoRead;
+    ftMaxCloseLoopFreqRead      : F_TRIG;
+    ftPositionerTypeRead        : F_TRIG;
+    ftCalibrationOptRead        : F_TRIG;
+    ftSensorPowerModeRead       : F_TRIG;
+    ftReferencingOptRead        : F_TRIG;
+    ftSafeDirRead               : F_TRIG;
+    ftMotorLoadRead             : F_TRIG;
+    ftChannelTypeRead           : F_TRIG;
+    ftChannelStateRead          : F_TRIG;
+    ftReferenceTypeRead         : F_TRIG;
+    ftLogicalScaleOffsetRead    : F_TRIG;
+    ftLogicalScaleInvRead       : F_TRIG;
+    tonSDORateLimit             : TON;
+    rtExecParamsRead            : R_TRIG;
+    bInternalChanReadTrigger    : BOOL;        // Internal trigger
+    tSDOInterval                : TIME := T#2S;
+    eSDOReadStep : INT := 0; // 0: Idle, 1: Wait-read, 2: Wait-ft, -1: Error
+END_VAR]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+IF bEnable THEN
+    CASE eSDOReadStep OF
+
+    0:
+        // Timer triggers if ALL reads are complete and (on init) axisParams are initialized
+        bChanReadTrigger := FALSE;
+        IF tonSDOReadTrigger.Q THEN
+            eSDOReadStep := 1;
+        END_IF
+
+    1: // Start SDO read (one-shot)
+        bChanReadTrigger := TRUE;
+        eSDOReadStep := 2;
+
+    2: // Wait for critical SDO read to complete
+        ftChannelStateRead(CLK := fbChannelStateRead.bBusy);
+
+        IF ftChannelStateRead.Q THEN
+            IF NOT fbChannelStateRead.bError THEN
+                stMCS2ChanParameter.stChannelState.bActivelyMoving        := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000001) <> 0;
+                stMCS2ChanParameter.stChannelState.bClosedLoopActive      := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000002) <> 0;
+                stMCS2ChanParameter.stChannelState.bCalibrating           := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000004) <> 0;
+                stMCS2ChanParameter.stChannelState.bReferencing           := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000008) <> 0;
+                stMCS2ChanParameter.stChannelState.bMoveDelayed           := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000010) <> 0;
+                stMCS2ChanParameter.stChannelState.bSensorPresent         := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000020) <> 0;
+                stMCS2ChanParameter.stChannelState.bIsCalibrated          := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000040) <> 0;
+                stMCS2ChanParameter.stChannelState.bIsReferenced          := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000080) <> 0;
+                stMCS2ChanParameter.stChannelState.bEndStopReached        := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000100) <> 0;
+                stMCS2ChanParameter.stChannelState.bRangeLimitReached     := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000200) <> 0;
+                stMCS2ChanParameter.stChannelState.bFollowingLimitReached := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000400) <> 0;
+                stMCS2ChanParameter.stChannelState.bMovementFailed        := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00000800) <> 0;
+                stMCS2ChanParameter.stChannelState.bIsStreaming           := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00001000) <> 0;
+                stMCS2ChanParameter.stChannelState.bPositionerOverload    := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00002000) <> 0;
+                stMCS2ChanParameter.stChannelState.bOverTemperature       := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00004000) <> 0;
+                stMCS2ChanParameter.stChannelState.bReferenceMark         := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00008000) <> 0;
+                stMCS2ChanParameter.stChannelState.bIsPhased              := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00010000) <> 0;
+                stMCS2ChanParameter.stChannelState.bPositionerFault       := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00020000) <> 0;
+                stMCS2ChanParameter.stChannelState.bAmplifierEnabled      := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00040000) <> 0;
+                stMCS2ChanParameter.stChannelState.bInPosition            := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00080000) <> 0;
+                stMCS2ChanParameter.stChannelState.bBrakeEnabled          := (stMCS2ChanParameter.stChannelState.nChannelStateRaw AND 16#00100000) <> 0;
+            ELSE
+                IF NOT stMotionStage.bBusy THEN
+                    stMotionStage.sErrorMessage := 'Failed to read ChannelState';
+                END_IF
+            END_IF
+            // RE-trigger the timer (restarts after SDO read is finished and bits decoded)
+            bChanReadTrigger := FALSE;
+            tonSDOReadTrigger(IN := TRUE);
+            eSDOReadStep := 0;
+        END_IF
+
+    ELSE
+        eSDOReadStep := 0;
+    END_CASE
+
+    IF eSDOReadStep = 0 THEN
+        tonSDOReadTrigger(
+            IN := stMotionStage.bAxisParamsInit,
+            PT := stMCS2ChanParameter.nReadTime
+        );
+    ELSE
+        tonSDOReadTrigger(
+            IN := FALSE,
+            PT := stMCS2ChanParameter.nReadTime
+        );
+    END_IF
+
+
+    fbMaxCloseLoopFreqRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanMaxCloseLoopFreqIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nMaxCloseLoopFreq),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nMaxCloseLoopFreq),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbPositionerTypeRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanPositionerTypeIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nPositionerType),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nPositionerType),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbCalibrationOptRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanCalibrationOptIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nCalibrationOpt),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nCalibrationOpt),
+        bExecute    := bChanReadTrigger
+    );
+    //
+    fbSensorPowerModeRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanSensorPowerModeIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nSensorPowerMode),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nSensorPowerMode),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbReferencingOptRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanReferencingOptIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nReferencingOpt),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nReferencingOpt),
+        bExecute    := bChanReadTrigger
+    );
+    //
+    fbReferenceTypeRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanReferenceTypeIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nReferenceType),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nReferenceType),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbSafeDirRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanSafeDirIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nSafeDir),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nSafeDir),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbMotorLoadRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanMotorLoadIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nMotorLoad),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nMotorLoad),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbChannelTypeRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChannelTypeIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nChannelType),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nChannelType),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbChannelStateRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanStateIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.stChannelState.nChannelStateRaw),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.stChannelState.nChannelStateRaw),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbLogicalScaleOffsetRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanLogicalScaleOfsIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nLogicalScaleOffset),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nLogicalScaleOffset),
+        bExecute    := bChanReadTrigger
+    );
+
+    fbLogicalScaleInvRead(
+        sNetId      := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr  := stDS402Drive.nSlaveAddr,
+        nIndex      := stMCS2ChanParameter.nChanLogicalScaleInvIdx,
+        nSubIndex   := 0,
+        pDstBuf     := ADR(stMCS2ChanParameter.nLogicalScaleInv),
+        cbBufLen    := SIZEOF(stMCS2ChanParameter.nLogicalScaleInv),
+        bExecute    := bChanReadTrigger
+    );
+
+END_IF]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadDriveCodes.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadDriveCodes.TcPOU
new file mode 100644
index 00000000..63c52136
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_ReadDriveCodes.TcPOU
@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_ReadDriveCodes" Id="{44ddb32d-aaa2-4fc7-a16a-bf90774280d7}" SpecialFunc="None">
+    <Declaration><![CDATA[(* Read Drive error codes after a motion or fault condition occured *)
+FUNCTION_BLOCK FB_ReadDriveCodes
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+    stMCS2ChanParameter	: ST_MCS2ChanParameter;
+END_VAR
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR
+    {attribute 'hide'}
+    fbErrorRead         : FB_EcCoESdoRead;
+    {attribute 'hide'}
+    nPiezoErrorCode     : UDINT;
+    {attribute 'hide'}
+    fbLogError          : FB_LogMotionError;
+    {attribute 'hide'}
+    ftErrorReadDone     : F_TRIG;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+fbErrorRead(
+    sNetId:= THIS^.stMotionStage.stAxisParameters.sAmsNetId,
+    nSlaveAddr:=stDS402Drive.nSlaveAddr,
+    nIndex:=stMCS2ChanParameter.nChanErrorCodeIdx,
+    nSubIndex :=0,
+    pDstBuf:= ADR(nPiezoErrorCode),
+    cbBufLen:=SIZEOF(nPiezoErrorCode),
+    bExecute:= stMotionStage.bError AND bEnable
+);
+
+ftErrorReadDone(CLK:=fbErrorRead.bBusy);
+IF ftErrorReadDone.Q AND bEnable THEN
+    IF  NOT fbErrorRead.bError AND nPiezoErrorCode <> 0 THEN
+        stMotionStage.nErrorId := nPiezoErrorCode;
+    END_IF
+    stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId:=stMotionStage.nErrorId);
+    fbLogError( stMotionStage:=stMotionStage, bEnable:=stMotionStage.bError);
+ELSE
+    // just return the NC error
+    stMotionStage.sErrorMessage := F_MotionErrorCodeLookup(nErrorId:=stMotionStage.nErrorId);
+    fbLogError( stMotionStage:=stMotionStage, bEnable:=stMotionStage.bError);
+END_IF
+]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteCalibrationParameters.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteCalibrationParameters.TcPOU
new file mode 100644
index 00000000..b90f3356
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteCalibrationParameters.TcPOU
@@ -0,0 +1,138 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_WriteCalibrationParameters" Id="{6f8141b3-3a18-4ecd-8d0f-d7b01f6561a0}" SpecialFunc="None">
+    <Declaration><![CDATA[// ===== Calibration Param SDO Hybrid Logic (startup-sequential, then on-demand) =====
+// Sequential trigger (bCalibParamStep statemachine) is used at startup or force, otherwise parallel/on-demand.
+
+FUNCTION_BLOCK FB_WriteCalibrationParameters
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+    stMCS2ChanParameter	: ST_MCS2ChanParameter;
+END_VAR
+
+VAR_OUTPUT
+   bCalibrationsParamsSet : BOOL;
+END_VAR
+VAR
+    {attribute 'hide'}
+    fbSafeDirWrite              : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbChannelTypeWrite          : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbPositionerTypeWrite       : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbCalibrationOptWrite       : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    ftPositionerTypeSetDone     : F_TRIG;
+    {attribute 'hide'}
+    ftCalibrationOptSetDone     : F_TRIG;
+    {attribute 'hide'}
+    ftSafeDirSetDone            : F_TRIG;
+    {attribute 'hide'}
+    ftChannelTypeSetDone        : F_TRIG;
+    {attribute 'hide'}
+    nRecentPositionerType       : UDINT;
+    {attribute 'hide'}
+    nRecentCalibrationOpt       : UDINT;
+    {attribute 'hide'}
+    nRecentSafeDir              : BOOL;
+    {attribute 'hide'}
+    nRecentChannelType          : UDINT;
+    tonStartUp : TON;
+    nCalibParamStep : INT := 0; // 0: idle, 1~5: write steps, 10: done, -1: error
+    bForceSequentialSDOs : BOOL := FALSE;
+    bErrorAcknowledge: BOOL;
+END_VAR
+
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+IF bEnable THEN
+    tonStartUp(
+        IN := bEnable AND NOT bCalibrationsParamsSet,
+        PT := T#4S
+    );
+
+    IF (NOT tonStartUp.Q) AND (nCalibParamStep <> 0) AND (NOT bForceSequentialSDOs) THEN
+        nCalibParamStep := 0;
+    END_IF
+
+    IF (nCalibParamStep = -1) AND bErrorAcknowledge THEN
+        nCalibParamStep := 0;
+        stMotionStage.bError := FALSE;
+        stMotionStage.nErrorId := 0;
+    END_IF
+
+    ftCalibrationOptSetDone(CLK := fbCalibrationOptWrite.bBusy);
+
+    IF tonStartUp.Q OR bForceSequentialSDOs THEN
+
+        CASE nCalibParamStep OF
+        0:
+            bCalibrationsParamsSet := FALSE;
+            fbCalibrationOptWrite.bExecute := FALSE;
+            nCalibParamStep := 1;
+
+        1:
+            IF (stMCS2ChanParameter.nCalibrationOpt <> nRecentCalibrationOpt) THEN
+                fbCalibrationOptWrite.bExecute := TRUE;
+                IF ftCalibrationOptSetDone.Q THEN
+                    IF fbCalibrationOptWrite.bError THEN
+                        stMotionStage.bError := fbCalibrationOptWrite.bError;
+                        stMotionStage.nErrorId := fbCalibrationOptWrite.nErrId;
+                        nCalibParamStep := -1;
+                    ELSE
+                        nRecentCalibrationOpt := stMCS2ChanParameter.nCalibrationOpt;
+                        fbCalibrationOptWrite.bExecute := FALSE;
+                        nCalibParamStep := 10;
+                    END_IF
+                END_IF
+            ELSE
+                nCalibParamStep := 10;
+            END_IF
+
+        10:
+            bCalibrationsParamsSet := TRUE;
+        -1:
+            bCalibrationsParamsSet := FALSE;
+        ELSE
+            nCalibParamStep := 0;
+        END_CASE
+    ELSE
+        fbCalibrationOptWrite.bExecute := (stMCS2ChanParameter.nCalibrationOpt <> nRecentCalibrationOpt);
+
+        IF ftCalibrationOptSetDone.Q THEN
+            IF fbCalibrationOptWrite.bError THEN
+                stMotionStage.bError := fbCalibrationOptWrite.bError;
+                stMotionStage.nErrorId := fbCalibrationOptWrite.nErrId;
+            ELSE
+                nRecentCalibrationOpt := stMCS2ChanParameter.nCalibrationOpt;
+                bCalibrationsParamsSet := TRUE;
+            END_IF
+            fbCalibrationOptWrite.bExecute := FALSE;
+        END_IF
+    END_IF
+
+    fbCalibrationOptWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanCalibrationOptIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nCalibrationOpt),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nCalibrationOpt)
+    );
+
+    bCalibrationsParamsSet R= fbCalibrationOptWrite.bBusy;
+END_IF]]></ST>
+    </Implementation>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteChanParameters.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteChanParameters.TcPOU
new file mode 100644
index 00000000..ff97c4bb
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteChanParameters.TcPOU
@@ -0,0 +1,403 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_WriteChanParameters" Id="{ba090cc0-f229-497a-a3d8-032477a19a94}" SpecialFunc="None">
+    <Declaration><![CDATA[// ===== Channel/Drive Params SDO Sequence (startup-safe hybrid) =====
+// Performs SDO writes sequentially at boot or when forced; uses parallel (on-demand) logic otherwise.
+
+FUNCTION_BLOCK FB_WriteChanParameters
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+    stMCS2ChanParameter	: ST_MCS2ChanParameter;
+END_VAR
+
+VAR_OUTPUT
+   bChanParamsSet : BOOL;
+END_VAR
+VAR
+    {attribute 'hide'}
+    fbSetFErrorWin              : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetSoftLimMin             : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetSoftLimMax             : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbMaxCloseLoopFreqWrite     : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSensorPowerModeWrite      : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbLogicalScaleOffsetWrite   : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbLogicalScaleInvWrite      : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    nRecentSoftLimMax           : LINT;
+    {attribute 'hide'}
+    nRecentSoftLimMin           : LINT;
+    {attribute 'hide'}
+    nRecentFErrWin              : DINT;
+
+    {attribute 'hide'}
+    nRecentMaxCloseLoopFreq     : UDINT;
+    {attribute 'hide'}
+    nRecentSensorPowerMode      : UDINT;
+    {attribute 'hide'}
+    nRecentLogicalScaleOffset   : LINT;
+    {attribute 'hide'}
+    nRecentLogicalScaleInv      : BOOL;
+    {attribute 'hide'}
+    ftFErrWinSetDone            : F_TRIG;
+    {attribute 'hide'}
+    ftSoftLimMaxSetDone         : F_TRIG;
+    {attribute 'hide'}
+    ftSoftLimMinSetDone         : F_TRIG;
+    {attribute 'hide'}
+    ftMaxCloseLoopFreqSetDone   : F_TRIG;
+    {attribute 'hide'}
+    ftSensorPowerModeSetDone    : F_TRIG;
+    {attribute 'hide'}
+    ftLogicalScaleOffsetSetDone : F_TRIG;
+    {attribute 'hide'}
+    ftLogicalScaleInvSetDone : F_TRIG;
+    bForceSequentialSDOs : BOOL := FALSE;
+    bErrorAcknowledge: BOOL;
+    tonStartUp : TON;
+    nChanParamStep : INT := 0; // Drive params: 0=idle, 1..7 steps, 10=done, -1=error
+    {attribute 'hide'}
+    nScalededStepAmp         : UINT;
+    {attribute 'hide'}
+    nScaledStepFreq          : UINT;
+{attribute 'hide'}
+    nScaledFErrWin          : DINT;
+    {attribute 'hide'}
+    nScalededSteps          : DINT;
+    {attribute 'hide'}
+    fScalededSteps          : LREAL;
+
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+IF bEnable THEN
+
+    tonStartUp(
+        IN := bEnable AND NOT bChanParamsSet,
+        PT := T#4S
+    );
+    IF (NOT tonStartUp.Q) AND (nChanParamStep <> 0) AND (NOT bForceSequentialSDOs) THEN
+        nChanParamStep := 0;
+    END_IF
+
+    IF (nChanParamStep = -1) AND bErrorAcknowledge THEN
+        nChanParamStep := 0;
+        stMotionStage.bError := FALSE;
+        stMotionStage.nErrorId := 0;
+    END_IF
+
+    ScaleDriveParams();
+
+    ftFErrWinSetDone              (CLK := fbSetFErrorWin.bBusy);
+    ftSoftLimMinSetDone           (CLK := fbSetSoftLimMin.bBusy);
+    ftSoftLimMaxSetDone           (CLK := fbSetSoftLimMax.bBusy);
+    ftMaxCloseLoopFreqSetDone     (CLK := fbMaxCloseLoopFreqWrite.bBusy);
+    ftSensorPowerModeSetDone      (CLK := fbSensorPowerModeWrite.bBusy);
+    ftLogicalScaleOffsetSetDone   (CLK := fbLogicalScaleOffsetWrite.bBusy);
+    ftLogicalScaleInvSetDone      (CLK := fbLogicalScaleInvWrite.bBusy);
+
+    IF tonStartUp.Q OR bForceSequentialSDOs THEN
+
+        CASE nChanParamStep OF
+        0:
+            bChanParamsSet := FALSE;
+            fbSetFErrorWin.bExecute            := FALSE;
+            fbSetSoftLimMin.bExecute           := FALSE;
+            fbSetSoftLimMax.bExecute           := FALSE;
+            fbMaxCloseLoopFreqWrite.bExecute   := FALSE;
+            fbSensorPowerModeWrite.bExecute    := FALSE;
+            fbLogicalScaleOffsetWrite.bExecute := FALSE;
+            fbLogicalScaleInvWrite.bExecute    := FALSE;
+            nChanParamStep := 1;
+
+        1: // Following Error Window
+            IF (nScaledFErrWin <> nRecentFErrWin) THEN
+                fbSetFErrorWin.bExecute := TRUE;
+                IF ftFErrWinSetDone.Q THEN
+                    IF fbSetFErrorWin.bError THEN
+                        stMotionStage.bError := fbSetFErrorWin.bError;
+                        stMotionStage.nErrorId := fbSetFErrorWin.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentFErrWin := nScaledFErrWin;
+                        fbSetFErrorWin.bExecute := FALSE;
+                        nChanParamStep := 2;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 2;
+            END_IF
+
+        2: // Soft Limit Min
+            IF (stMCS2ChanParameter.nSoftLimMin <> nRecentSoftLimMin) THEN
+                fbSetSoftLimMin.bExecute := TRUE;
+                IF ftSoftLimMinSetDone.Q THEN
+                    IF fbSetSoftLimMin.bError THEN
+                        stMotionStage.bError := fbSetSoftLimMin.bError;
+                        stMotionStage.nErrorId := fbSetSoftLimMin.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentSoftLimMin := stMCS2ChanParameter.nSoftLimMin;
+                        fbSetSoftLimMin.bExecute := FALSE;
+                        nChanParamStep := 3;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 3;
+            END_IF
+
+        3: // Soft Limit Max
+            IF (stMCS2ChanParameter.nSoftLimMax <> nRecentSoftLimMax) THEN
+                fbSetSoftLimMax.bExecute := TRUE;
+                IF ftSoftLimMaxSetDone.Q THEN
+                    IF fbSetSoftLimMax.bError THEN
+                        stMotionStage.bError := fbSetSoftLimMax.bError;
+                        stMotionStage.nErrorId := fbSetSoftLimMax.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentSoftLimMax := stMCS2ChanParameter.nSoftLimMax;
+                        fbSetSoftLimMax.bExecute := FALSE;
+                        nChanParamStep := 4;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 4;
+            END_IF
+
+        4: // Max Close Loop Freq
+            IF (stMCS2ChanParameter.nMaxCloseLoopFreq <> nRecentMaxCloseLoopFreq) THEN
+                fbMaxCloseLoopFreqWrite.bExecute := TRUE;
+                IF ftMaxCloseLoopFreqSetDone.Q THEN
+                    IF fbMaxCloseLoopFreqWrite.bError THEN
+                        stMotionStage.bError := fbMaxCloseLoopFreqWrite.bError;
+                        stMotionStage.nErrorId := fbMaxCloseLoopFreqWrite.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentMaxCloseLoopFreq := stMCS2ChanParameter.nMaxCloseLoopFreq;
+                        fbMaxCloseLoopFreqWrite.bExecute := FALSE;
+                        nChanParamStep := 5;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 5;
+            END_IF
+
+        5: // Sensor Power Mode
+            IF (stMCS2ChanParameter.nSensorPowerMode <> nRecentSensorPowerMode) THEN
+                fbSensorPowerModeWrite.bExecute := TRUE;
+                IF ftSensorPowerModeSetDone.Q THEN
+                    IF fbSensorPowerModeWrite.bError THEN
+                        stMotionStage.bError := fbSensorPowerModeWrite.bError;
+                        stMotionStage.nErrorId := fbSensorPowerModeWrite.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentSensorPowerMode := stMCS2ChanParameter.nSensorPowerMode;
+                        fbSensorPowerModeWrite.bExecute := FALSE;
+                        nChanParamStep := 6;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 6;
+            END_IF
+
+        6: // Logical Scale Offset
+            IF (stMCS2ChanParameter.nLogicalScaleOffset <> nRecentLogicalScaleOffset) THEN
+                fbLogicalScaleOffsetWrite.bExecute := TRUE;
+                IF ftLogicalScaleOffsetSetDone.Q THEN
+                    IF fbLogicalScaleOffsetWrite.bError THEN
+                        stMotionStage.bError := fbLogicalScaleOffsetWrite.bError;
+                        stMotionStage.nErrorId := fbLogicalScaleOffsetWrite.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentLogicalScaleOffset := stMCS2ChanParameter.nLogicalScaleOffset;
+                        fbLogicalScaleOffsetWrite.bExecute := FALSE;
+                        nChanParamStep := 7;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 7;
+            END_IF
+
+        7: // Logical Scale Inv
+            IF (stMCS2ChanParameter.nLogicalScaleInv <> nRecentLogicalScaleInv) THEN
+                fbLogicalScaleInvWrite.bExecute := TRUE;
+                IF ftLogicalScaleInvSetDone.Q THEN
+                    IF fbLogicalScaleInvWrite.bError THEN
+                        stMotionStage.bError := fbLogicalScaleInvWrite.bError;
+                        stMotionStage.nErrorId := fbLogicalScaleInvWrite.nErrId;
+                        nChanParamStep := -1;
+                    ELSE
+                        nRecentLogicalScaleInv := stMCS2ChanParameter.nLogicalScaleInv;
+                        fbLogicalScaleInvWrite.bExecute := FALSE;
+                        nChanParamStep := 10;
+                    END_IF
+                END_IF
+            ELSE
+                nChanParamStep := 10;
+            END_IF
+
+        10: // Done
+            bChanParamsSet := TRUE;
+        -1:
+            bChanParamsSet := FALSE;
+        ELSE
+            nChanParamStep := 0;
+        END_CASE
+
+    ELSE
+        fbSetFErrorWin.bExecute            := (nScaledFErrWin <> nRecentFErrWin);
+        fbSetSoftLimMin.bExecute           := (stMCS2ChanParameter.nSoftLimMin <> nRecentSoftLimMin);
+        fbSetSoftLimMax.bExecute           := (stMCS2ChanParameter.nSoftLimMax <> nRecentSoftLimMax);
+        fbMaxCloseLoopFreqWrite.bExecute   := (stMCS2ChanParameter.nMaxCloseLoopFreq <> nRecentMaxCloseLoopFreq);
+        fbSensorPowerModeWrite.bExecute    := (stMCS2ChanParameter.nSensorPowerMode <> nRecentSensorPowerMode);
+        fbLogicalScaleOffsetWrite.bExecute := (stMCS2ChanParameter.nLogicalScaleOffset <> nRecentLogicalScaleOffset);
+        fbLogicalScaleInvWrite.bExecute    := (stMCS2ChanParameter.nLogicalScaleInv <> nRecentLogicalScaleInv);
+
+        IF ftFErrWinSetDone.Q OR ftSoftLimMinSetDone.Q OR ftSoftLimMaxSetDone.Q OR
+           ftMaxCloseLoopFreqSetDone.Q OR ftSensorPowerModeSetDone.Q OR
+           ftLogicalScaleOffsetSetDone.Q OR ftLogicalScaleInvSetDone.Q THEN
+
+            IF fbSetFErrorWin.bError THEN
+                stMotionStage.bError := fbSetFErrorWin.bError;
+                stMotionStage.nErrorId := fbSetFErrorWin.nErrId;
+            ELSIF fbSetSoftLimMin.bError THEN
+                stMotionStage.bError := fbSetSoftLimMin.bError;
+                stMotionStage.nErrorId := fbSetSoftLimMin.nErrId;
+            ELSIF fbSetSoftLimMax.bError THEN
+                stMotionStage.bError := fbSetSoftLimMax.bError;
+                stMotionStage.nErrorId := fbSetSoftLimMax.nErrId;
+            ELSIF fbMaxCloseLoopFreqWrite.bError THEN
+                stMotionStage.bError := fbMaxCloseLoopFreqWrite.bError;
+                stMotionStage.nErrorId := fbMaxCloseLoopFreqWrite.nErrId;
+            ELSIF fbSensorPowerModeWrite.bError THEN
+                stMotionStage.bError := fbSensorPowerModeWrite.bError;
+                stMotionStage.nErrorId := fbSensorPowerModeWrite.nErrId;
+            ELSIF fbLogicalScaleOffsetWrite.bError THEN
+                stMotionStage.bError := fbLogicalScaleOffsetWrite.bError;
+                stMotionStage.nErrorId := fbLogicalScaleOffsetWrite.nErrId;
+            ELSIF fbLogicalScaleInvWrite.bError THEN
+                stMotionStage.bError := fbLogicalScaleInvWrite.bError;
+                stMotionStage.nErrorId := fbLogicalScaleInvWrite.nErrId;
+            ELSE
+                nRecentFErrWin           := nScaledFErrWin;
+                nRecentSoftLimMin        := stMCS2ChanParameter.nSoftLimMin;
+                nRecentSoftLimMax        := stMCS2ChanParameter.nSoftLimMax;
+                nRecentMaxCloseLoopFreq  := stMCS2ChanParameter.nMaxCloseLoopFreq;
+                nRecentSensorPowerMode   := stMCS2ChanParameter.nSensorPowerMode;
+                nRecentLogicalScaleOffset:= stMCS2ChanParameter.nLogicalScaleOffset;
+                nRecentLogicalScaleInv   := stMCS2ChanParameter.nLogicalScaleInv;
+                bChanParamsSet := TRUE;
+            END_IF
+
+            fbSetFErrorWin.bExecute            := FALSE;
+            fbSetSoftLimMin.bExecute           := FALSE;
+            fbSetSoftLimMax.bExecute           := FALSE;
+            fbMaxCloseLoopFreqWrite.bExecute   := FALSE;
+            fbSensorPowerModeWrite.bExecute    := FALSE;
+            fbLogicalScaleOffsetWrite.bExecute := FALSE;
+            fbLogicalScaleInvWrite.bExecute    := FALSE;
+        END_IF
+    END_IF
+
+    fbSetFErrorWin(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanFErrWinIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(nScaledFErrWin),
+        cbBufLen   := SIZEOF(nScaledFErrWin)
+    );
+    fbSetSoftLimMin(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanSoftLimitIdx,
+        nSubIndex  := 1,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nSoftLimMin),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nSoftLimMin)
+    );
+    fbSetSoftLimMax(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanSoftLimitIdx,
+        nSubIndex  := 2,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nSoftLimMax),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nSoftLimMax)
+    );
+    fbMaxCloseLoopFreqWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanMaxCloseLoopFreqIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nMaxCloseLoopFreq),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nMaxCloseLoopFreq)
+    );
+    fbSensorPowerModeWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanSensorPowerModeIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nSensorPowerMode),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nSensorPowerMode)
+    );
+    fbLogicalScaleOffsetWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanLogicalScaleOfsIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nLogicalScaleOffset),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nLogicalScaleOffset)
+    );
+    fbLogicalScaleInvWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanLogicalScaleInvIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nLogicalScaleInv),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nLogicalScaleInv)
+    );
+
+    // Set a busy flag for status
+    bChanParamsSet R= fbSetFErrorWin.bBusy
+        OR fbSetSoftLimMin.bBusy
+        OR fbSetSoftLimMax.bBusy
+        OR fbMaxCloseLoopFreqWrite.bBusy
+        OR fbSensorPowerModeWrite.bBusy
+        OR fbLogicalScaleOffsetWrite.bBusy
+        OR fbLogicalScaleInvWrite.bBusy;
+END_IF]]></ST>
+    </Implementation>
+    <Method Name="ScaleDriveParams" Id="{d0da9ee6-ac11-4834-86fa-47327c456911}">
+      <Declaration><![CDATA[METHOD ScaleDriveParams
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[nScaledFErrWin:=LREAL_TO_DINT(( stMotionStage.stAxisParameters.fCtrlPosDiffMax / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+
+IF  stMotionStage.stAxisParameters.bEncEnableSoftEndMinControl.0 AND  stMotionStage.stAxisParameters.bEncEnableSoftEndMaxControl.0 THEN
+    stMCS2ChanParameter.nSoftLimMax := LREAL_TO_DINT(( stMotionStage.stAxisParameters.fEncSoftEndMax / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+    stMCS2ChanParameter.nSoftLimMin := LREAL_TO_DINT(( stMotionStage.stAxisParameters.fEncSoftEndMin / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+ELSE
+    // Soft limit disabled
+    stMCS2ChanParameter.nSoftLimMax := 0;
+    stMCS2ChanParameter.nSoftLimMin := 0;
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteHomeParameters.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteHomeParameters.TcPOU
new file mode 100644
index 00000000..8f72e35e
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteHomeParameters.TcPOU
@@ -0,0 +1,299 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_WriteHomeParameters" Id="{e85d8b4a-38f0-4a4c-8985-f84fb315cac2}" SpecialFunc="None">
+    <Declaration><![CDATA[// The SDO writes can be done sequentially at boot or on demand during normal use.
+// Set bForceSequentialSDOs TRUE (debug) to enforce sequential behavior at any time.
+FUNCTION_BLOCK FB_WriteHomeParameters
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+    stMCS2ChanParameter	: ST_MCS2ChanParameter;
+END_VAR
+
+VAR_OUTPUT
+   bHomingParamsSet : BOOL;
+END_VAR
+VAR
+    nHomingStep : INT := 0; // 0: idle, 1~5: write steps, 10: done, -1: error
+    bForceSequentialSDOs : BOOL := FALSE;
+    {attribute 'hide'}
+    fbSetHomeOffs               : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetHomeVeloFast           : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetHomeVeloSlow           : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetHomeAcc                : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbReferencingOptWrite       : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    ftHomeVeloFastSetDone       : F_TRIG;
+    {attribute 'hide'}
+    ftHomeAccSetDone            : F_TRIG;
+    {attribute 'hide'}
+    ftHomeOffsSetDone           : F_TRIG;
+    {attribute 'hide'}
+    ftHomeVeloSlowSetDone       : F_TRIG;
+    {attribute 'hide'}
+    ftReferencingOptSetDone     : F_TRIG;
+
+    {attribute 'hide'}
+    nRecentHomeVeloFast         : UDINT;
+    {attribute 'hide'}
+    nRecentHomeVeloSlow         : UDINT;
+    {attribute 'hide'}
+    nRecentHomeAcc              : UDINT;
+    {attribute 'hide'}
+    nRecentHomeOffset           : DINT;
+    {attribute 'hide'}
+    nRecentReferencingOpt       : UDINT;
+    {attribute 'hide'}
+    nHomeOffs                   : DINT;
+    tonStartUp : TON;
+    bErrorAcknowledge: BOOL;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+IF bEnable THEN
+    tonStartUp(
+        IN := bEnable AND NOT bHomingParamsSet,
+        PT := T#4S
+    );
+
+    IF (NOT tonStartUp.Q) AND (nHomingStep <> 0) AND (NOT bForceSequentialSDOs) THEN
+        nHomingStep := 0;
+    END_IF
+
+    IF (nHomingStep = -1) AND bErrorAcknowledge THEN
+        nHomingStep := 0;
+        stMotionStage.bError := FALSE;
+        stMotionStage.nErrorId := 0;
+        bErrorAcknowledge := FALSE;
+    END_IF
+
+    ScaleHomingParams();
+    ftHomeVeloFastSetDone(CLK:=fbSetHomeVeloFast.bBusy);
+    ftHomeVeloSlowSetDone(CLK:=fbSetHomeVeloSlow.bBusy);
+    ftHomeAccSetDone(CLK:=fbSetHomeAcc.bBusy);
+    ftHomeOffsSetDone(CLK:=fbSetHomeOffs.bBusy);
+    ftReferencingOptSetDone(CLK:=fbReferencingOptWrite.bBusy);
+
+    IF tonStartUp.Q OR bForceSequentialSDOs THEN
+
+        CASE nHomingStep OF
+
+        0:
+            bHomingParamsSet := FALSE;
+            fbSetHomeVeloFast.bExecute := FALSE;
+            fbSetHomeVeloSlow.bExecute := FALSE;
+            fbSetHomeAcc.bExecute := FALSE;
+            fbSetHomeOffs.bExecute := FALSE;
+            fbReferencingOptWrite.bExecute := FALSE;
+            nHomingStep := 1;
+
+        1: // Write Home Velo Fast
+            IF (stMCS2ChanParameter.nHomeVeloFast <> nRecentHomeVeloFast) THEN
+                fbSetHomeVeloFast.bExecute := TRUE;
+                IF ftHomeVeloFastSetDone.Q  THEN
+                    IF fbSetHomeVeloFast.bError THEN
+                         stMotionStage.bError := fbSetHomeVeloFast.bError;
+                         stMotionStage.nErrorId := fbSetHomeVeloFast.nErrId;
+                         nHomingStep := -1;
+                    ELSE
+                        nRecentHomeVeloFast      := stMCS2ChanParameter.nHomeVeloFast;
+                        bHomingParamsSet := TRUE;
+                    END_IF
+                    fbSetHomeVeloFast.bExecute := FALSE;
+                    nHomingStep := 2;
+                END_IF
+            ELSE
+                nHomingStep := 2;
+            END_IF
+
+        2: // Write Home Velo Slow
+            IF (stMCS2ChanParameter.nHomeVeloSlow <> nRecentHomeVeloSlow) THEN
+                fbSetHomeVeloSlow.bExecute := TRUE;
+
+                IF fbSetHomeVeloSlow.bError THEN
+                         stMotionStage.bError := fbSetHomeVeloSlow.bError;
+                         stMotionStage.nErrorId := fbSetHomeVeloSlow.nErrId;
+                         nHomingStep := -1;
+                ELSE
+                        nRecentHomeVeloSlow      := stMCS2ChanParameter.nHomeVeloSlow;
+                        bHomingParamsSet := TRUE;
+                END_IF
+                    fbSetHomeVeloSlow.bExecute := FALSE;
+                    nHomingStep := 3;
+            ELSE
+                nHomingStep := 3;
+            END_IF
+
+        3: // Write Home Acc
+            IF (stMCS2ChanParameter.nHomeAcc <> nRecentHomeAcc) THEN
+                fbSetHomeAcc.bExecute := TRUE;
+                IF fbSetHomeAcc.bError THEN
+                         stMotionStage.bError := fbSetHomeAcc.bError;
+                         stMotionStage.nErrorId := fbSetHomeAcc.nErrId;
+                         nHomingStep := -1;
+                ELSE
+                        nRecentHomeAcc     := stMCS2ChanParameter.nHomeAcc;
+                        bHomingParamsSet := TRUE;
+                END_IF
+                    fbSetHomeAcc.bExecute := FALSE;
+                    nHomingStep := 4;
+            ELSE
+                nHomingStep := 4;
+            END_IF
+
+        4: // Write Home Offset
+            IF (stMCS2ChanParameter.nHomeOffset <> nRecentHomeOffset) THEN
+                fbSetHomeOffs.bExecute := TRUE;
+                IF fbSetHomeOffs.bError THEN
+                         stMotionStage.bError := fbSetHomeOffs.bError;
+                         stMotionStage.nErrorId := fbSetHomeOffs.nErrId;
+                         nHomingStep := -1;
+                ELSE
+                        nRecentHomeOffset  := stMCS2ChanParameter.nHomeOffset;
+                        bHomingParamsSet := TRUE;
+                END_IF
+                    fbSetHomeAcc.bExecute := FALSE;
+                    nHomingStep := 5;
+            ELSE
+                nHomingStep := 5;
+            END_IF
+
+        5: // Write Referencing Option
+            IF (stMCS2ChanParameter.nReferencingOpt <> nRecentReferencingOpt) THEN
+                fbReferencingOptWrite.bExecute := TRUE;
+                IF fbSetHomeOffs.bError THEN
+                    stMotionStage.bError := fbReferencingOptWrite.bError;
+                    stMotionStage.nErrorId := fbReferencingOptWrite.nErrId;
+                    nHomingStep := -1;
+                ELSE
+                    nRecentReferencingOpt := stMCS2ChanParameter.nReferencingOpt;
+                    bHomingParamsSet := TRUE;
+                END_IF
+                    fbSetHomeAcc.bExecute := FALSE;
+                    nHomingStep := 10;
+            ELSE
+                nHomingStep := 10;
+            END_IF
+
+        10: // All done
+            bHomingParamsSet := TRUE;
+        -1: // Error state
+            bHomingParamsSet := FALSE;
+        ELSE
+            nHomingStep := 0;
+        END_CASE
+    ELSE
+        fbSetHomeVeloFast.bExecute := (stMCS2ChanParameter.nHomeVeloFast <> nRecentHomeVeloFast);
+        fbSetHomeVeloSlow.bExecute := (stMCS2ChanParameter.nHomeVeloSlow <> nRecentHomeVeloSlow);
+        fbSetHomeAcc.bExecute      := (stMCS2ChanParameter.nHomeAcc      <> nRecentHomeAcc);
+        fbSetHomeOffs.bExecute     := (stMCS2ChanParameter.nHomeOffset   <> nRecentHomeOffset);
+        fbReferencingOptWrite.bExecute := (stMCS2ChanParameter.nReferencingOpt <> nRecentReferencingOpt);
+
+        IF ftHomeVeloFastSetDone.Q OR ftHomeVeloSlowSetDone.Q OR ftHomeAccSetDone.Q
+           OR ftHomeOffsSetDone.Q OR ftReferencingOptSetDone.Q THEN
+            IF fbSetHomeVeloFast.bError THEN
+                stMotionStage.bError := fbSetHomeVeloFast.bError;
+                stMotionStage.nErrorId := fbSetHomeVeloFast.nErrId;
+            ELSIF fbSetHomeVeloSlow.bError THEN
+                stMotionStage.bError := fbSetHomeVeloSlow.bError;
+                stMotionStage.nErrorId := fbSetHomeVeloSlow.nErrId;
+            ELSIF fbSetHomeAcc.bError THEN
+                stMotionStage.bError := fbSetHomeAcc.bError;
+                stMotionStage.nErrorId := fbSetHomeAcc.nErrId;
+            ELSIF fbSetHomeOffs.bError THEN
+                stMotionStage.bError := fbSetHomeOffs.bError;
+                stMotionStage.nErrorId := fbSetHomeOffs.nErrId;
+            ELSIF fbReferencingOptWrite.bError THEN
+                stMotionStage.bError := fbReferencingOptWrite.bError;
+                stMotionStage.nErrorId := fbReferencingOptWrite.nErrId;
+            ELSE
+                nRecentHomeVeloFast      := stMCS2ChanParameter.nHomeVeloFast;
+                nRecentHomeVeloSlow      := stMCS2ChanParameter.nHomeVeloSlow;
+                nRecentHomeAcc           := stMCS2ChanParameter.nHomeAcc;
+                nRecentHomeOffset        := stMCS2ChanParameter.nHomeOffset;
+                nRecentReferencingOpt    := stMCS2ChanParameter.nReferencingOpt;
+                bHomingParamsSet := TRUE;
+            END_IF
+            fbSetHomeVeloFast.bExecute      := FALSE;
+            fbSetHomeVeloSlow.bExecute      := FALSE;
+            fbSetHomeAcc.bExecute           := FALSE;
+            fbSetHomeOffs.bExecute          := FALSE;
+            fbReferencingOptWrite.bExecute  := FALSE;
+        END_IF
+    END_IF
+
+    fbSetHomeVeloFast(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanHomeVeloIdx,
+        nSubIndex  := 1,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nHomeVeloFast),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nHomeVeloFast)
+    );
+    fbSetHomeVeloSlow(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanHomeVeloIdx,
+        nSubIndex  := 2,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nHomeVeloSlow),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nHomeVeloSlow)
+    );
+    fbSetHomeAcc(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanhomeAccIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nHomeAcc),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nHomeAcc)
+    );
+    fbSetHomeOffs(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanHomeOffsIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nHomeOffset),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nHomeOffset)
+    );
+    fbReferencingOptWrite(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanReferencingOptIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(stMCS2ChanParameter.nReferencingOpt),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nReferencingOpt)
+    );
+
+    bHomingParamsSet R= fbSetHomeVeloFast.bBusy
+        OR fbSetHomeVeloSlow.bBusy
+        OR fbSetHomeAcc.bBusy
+        OR fbSetHomeOffs.bBusy
+        OR fbReferencingOptWrite.bBusy;
+END_IF]]></ST>
+    </Implementation>
+    <Method Name="ScaleHomingParams" Id="{2c5b56b0-7eaa-4a13-8084-de781b4be994}">
+      <Declaration><![CDATA[(*Scale Home Move parameters*)
+METHOD ScaleHomingParams
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[stMCS2ChanParameter.nHomeVeloFast := LREAL_TO_UDINT(( stMotionStage.stAxisParameters.fRefVeloSearch / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+stMCS2ChanParameter.nHomeVeloSlow := LREAL_TO_UDINT(( stMotionStage.stAxisParameters.fRefVeloSync / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+stMCS2ChanParameter.nHomeAcc := LREAL_TO_UDINT((stMCS2ChanParameter.fHomeAcc / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+stMCS2ChanParameter.nHomeOffset := LREAL_TO_DINT(( stMotionStage.fHomePosition / MAX( stMotionStage.stAxisParameters.fEncScaleFactorInternal, stMCS2ChanParameter.fScalingFactor)));
+]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU
new file mode 100644
index 00000000..f01bf587
--- /dev/null
+++ b/lcls-twincat-motion/Library/POUs/Motion/DS402/MCS2/FB_WriteStepModeParameters.TcPOU
@@ -0,0 +1,236 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <POU Name="FB_WriteStepModeParameters" Id="{309453e5-6198-4e60-a7a5-fef2f5a9da17}" SpecialFunc="None">
+    <Declaration><![CDATA[// Sequential SDOs at startup or when forced; parallel writes otherwise (mirrors Homing Params block).
+FUNCTION_BLOCK FB_WriteStepModeParameters
+VAR_INPUT
+    bEnable : BOOL;
+END_VAR
+VAR_IN_OUT
+    stDS402Drive         : ST_DS402Drive;
+    stMotionStage        : ST_MotionStage;
+    stMCS2ChanParameter	: ST_MCS2ChanParameter;
+END_VAR
+
+VAR_OUTPUT
+   bStepModeParamsSet : BOOL;
+END_VAR
+VAR
+    // ====== Measurements & scaled values ======
+    {attribute 'hide'}
+    nScaledFErrWin          : DINT;
+    {attribute 'hide'}
+    nScalededSteps          : DINT;
+    {attribute 'hide'}
+    fScalededSteps          : LREAL;
+    {attribute 'hide'}
+    fMeasuredVelo           : LREAL;
+    {attribute 'hide'}
+    fMeasuredAcc            : LREAL;
+    {attribute 'hide'}
+    fParameterValue         : LREAL;
+    {attribute 'hide'}
+    fMeasuredPos             : LREAL;
+    // ====== Internal numeric parameters ======
+    {attribute 'hide'}
+    nScalededStepAmp         : UINT;
+    {attribute 'hide'}
+    nScaledStepFreq          : UINT;
+    // Open Loop Operations
+    {attribute 'hide'}
+    fbSetSteps          : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetStepFreq       : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    fbSetStepAmp        : FB_EcCoESdoWrite;
+    {attribute 'hide'}
+    nRecentSteps        : DINT;
+    {attribute 'hide'}
+    nRecentStepAmp      : UINT;
+    {attribute 'hide'}
+    nRecentStepFreq     : UINT;
+    {attribute 'hide'}
+    ftStepAmpUpdateDone : F_TRIG;
+    {attribute 'hide'}
+    ftStepFreqUpdateDone    : F_TRIG;
+    {attribute 'hide'}
+    ftStepUpdateDone    : F_TRIG;
+    tonStartUp : TON;
+    nJogParamStep : INT := 0; // 0=idle/start, 1=steps, 2=amp, 3=freq, 10=done, -1=error
+    bForceSequentialSDOs : BOOL := FALSE;
+    bErrorAcknowledge: BOOL;
+END_VAR
+]]></Declaration>
+    <Implementation>
+      <ST><![CDATA[IF NOT __ISVALIDREF(stDS402Drive)
+    OR NOT __ISVALIDREF(stMotionStage)
+    OR NOT __ISVALIDREF(stMCS2ChanParameter) THEN
+   RETURN;
+END_IF
+
+IF bEnable THEN
+    tonStartUp(
+        IN := bEnable AND NOT bStepModeParamsSet,
+        PT := T#4S
+    );
+    IF (NOT tonStartUp.Q) AND (nJogParamStep <> 0) AND (NOT bForceSequentialSDOs) THEN
+        nJogParamStep := 0;
+    END_IF
+
+    IF (nJogParamStep = -1) AND bErrorAcknowledge THEN
+        nJogParamStep := 0;
+        stMotionStage.bError := FALSE;
+        stMotionStage.nErrorId := 0;
+    END_IF
+    ScaleStepModeParams();
+
+    ftStepUpdateDone   (CLK := fbSetSteps.bBusy);
+    ftStepAmpUpdateDone(CLK := fbSetStepAmp.bBusy);
+    ftStepFreqUpdateDone(CLK := fbSetStepFreq.bBusy);
+
+    IF tonStartUp.Q OR bForceSequentialSDOs THEN
+        CASE nJogParamStep OF
+        0:
+            bStepModeParamsSet := FALSE;
+            fbSetSteps.bExecute     := FALSE;
+            fbSetStepAmp.bExecute   := FALSE;
+            fbSetStepFreq.bExecute  := FALSE;
+            nJogParamStep := 1;
+
+        1: // Set Steps
+            IF (nScalededSteps <> nRecentSteps) THEN
+                fbSetSteps.bExecute := TRUE;
+                IF ftStepUpdateDone.Q THEN
+                    IF fbSetSteps.bError THEN
+                        stMotionStage.bError := fbSetSteps.bError;
+                        stMotionStage.nErrorId := fbSetSteps.nErrId;
+                        nJogParamStep := -1;
+                    ELSE
+                        nRecentSteps := nScalededSteps;
+                        fbSetSteps.bExecute := FALSE;
+                        nJogParamStep := 2;
+                    END_IF
+                END_IF
+            ELSE
+                nJogParamStep := 2;
+            END_IF
+
+        2: // Set Step Amp
+            IF (nScalededStepAmp <> nRecentStepAmp) THEN
+                fbSetStepAmp.bExecute := TRUE;
+                IF ftStepAmpUpdateDone.Q THEN
+                    IF fbSetStepAmp.bError THEN
+                        stMotionStage.bError := fbSetStepAmp.bError;
+                        stMotionStage.nErrorId := fbSetStepAmp.nErrId;
+                        nJogParamStep := -1;
+                    ELSE
+                        nRecentStepAmp := nScalededStepAmp;
+                        fbSetStepAmp.bExecute := FALSE;
+                        nJogParamStep := 3;
+                    END_IF
+                END_IF
+            ELSE
+                nJogParamStep := 3;
+            END_IF
+
+        3: // Set Step Frequency
+            IF (nScaledStepFreq <> nRecentStepFreq) THEN
+                fbSetStepFreq.bExecute := TRUE;
+                IF ftStepFreqUpdateDone.Q THEN
+                    IF fbSetStepFreq.bError THEN
+                        stMotionStage.bError := fbSetStepFreq.bError;
+                        stMotionStage.nErrorId := fbSetStepFreq.nErrId;
+                        nJogParamStep := -1;
+                    ELSE
+                        nRecentStepFreq := nScaledStepFreq;
+                        fbSetStepFreq.bExecute := FALSE;
+                        nJogParamStep := 10;
+                    END_IF
+                END_IF
+            ELSE
+                nJogParamStep := 10;
+            END_IF
+
+        10: // Done
+            bStepModeParamsSet := TRUE;
+        -1: // Error state
+            bStepModeParamsSet := FALSE;
+        ELSE
+            nJogParamStep := 0;
+        END_CASE
+
+    ELSE
+        fbSetSteps.bExecute    := (nScalededSteps   <> nRecentSteps);
+        fbSetStepAmp.bExecute  := (nScalededStepAmp <> nRecentStepAmp);
+        fbSetStepFreq.bExecute := (nScaledStepFreq  <> nRecentStepFreq);
+
+        IF ftStepUpdateDone.Q OR ftStepAmpUpdateDone.Q OR ftStepFreqUpdateDone.Q THEN
+            IF fbSetSteps.bError THEN
+                stMotionStage.bError := fbSetSteps.bError;
+                stMotionStage.nErrorId := fbSetSteps.nErrId;
+            ELSIF fbSetStepAmp.bError THEN
+                stMotionStage.bError := fbSetStepAmp.bError;
+                stMotionStage.nErrorId := fbSetStepAmp.nErrId;
+            ELSIF fbSetStepFreq.bError THEN
+                stMotionStage.bError := fbSetStepFreq.bError;
+                stMotionStage.nErrorId := fbSetStepFreq.nErrId;
+            ELSE
+                nRecentSteps    := nScalededSteps;
+                nRecentStepAmp  := nScalededStepAmp;
+                nRecentStepFreq := nScaledStepFreq;
+                bStepModeParamsSet := TRUE;
+            END_IF
+
+            fbSetSteps.bExecute    := FALSE;
+            fbSetStepAmp.bExecute  := FALSE;
+            fbSetStepFreq.bExecute := FALSE;
+        END_IF
+    END_IF
+
+    fbSetSteps(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanStepIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(nScalededSteps),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nChanStep)
+    );
+    fbSetStepAmp(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanStepAmpIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(nScalededStepAmp),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nChanStepAmp)
+    );
+    fbSetStepFreq(
+        sNetId     := stMotionStage.stAxisParameters.sAmsNetId,
+        nSlaveAddr := stDS402Drive.nSlaveAddr,
+        nIndex     := stMCS2ChanParameter.nChanStepFreqIdx,
+        nSubIndex  := 0,
+        pSrcBuf    := ADR(nScaledStepFreq),
+        cbBufLen   := SIZEOF(stMCS2ChanParameter.nChanStepFreq)
+    );
+
+    bStepModeParamsSet R= fbSetSteps.bBusy
+        OR fbSetStepAmp.bBusy
+        OR fbSetStepFreq.bBusy;
+END_IF]]></ST>
+    </Implementation>
+    <Method Name="ScaleStepModeParams" Id="{78ce5a79-ef3b-44af-a794-7ac04154475b}">
+      <Declaration><![CDATA[(*Scaled Open Loop Step mode motion parameters*)
+METHOD ScaleStepModeParams
+VAR_INPUT
+END_VAR
+]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[//827.31x-837.63
+nScalededSteps:=LIMIT(-1000, stMCS2ChanParameter.nChanStep, 1000);
+nScalededStepAmp:=REAL_TO_UINT(LIMIT(50, stMCS2ChanParameter.nChanStepAmp, 100) * 655.35);
+nScaledStepFreq:=REAL_TO_UINT(LIMIT(500, stMCS2ChanParameter.nChanStepFreq, 1000.0));
+
+]]></ST>
+      </Implementation>
+    </Method>
+  </POU>
+</TcPlcObject>
\ No newline at end of file
diff --git a/lcls-twincat-motion/Library/_CompileInfo_Upload/AB9F1DA2-32AF-AFB4-FE1B-87E42F17C5BE.compileinfo b/lcls-twincat-motion/Library/_CompileInfo_Upload/AB9F1DA2-32AF-AFB4-FE1B-87E42F17C5BE.compileinfo
new file mode 100644
index 00000000..a880ae7e
Binary files /dev/null and b/lcls-twincat-motion/Library/_CompileInfo_Upload/AB9F1DA2-32AF-AFB4-FE1B-87E42F17C5BE.compileinfo differ
diff --git a/lcls-twincat-motion/_Config/NC/NC.xti b/lcls-twincat-motion/_Config/NC/NC.xti
index 346525db..95a2fa55 100644
--- a/lcls-twincat-motion/_Config/NC/NC.xti
+++ b/lcls-twincat-motion/_Config/NC/NC.xti
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<TcSmItem xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://www.beckhoff.com/schemas/2012/07/TcSmProject" TcSmVersion="1.0" TcVersion="3.1.4026.13" ClassName="CNcSafTaskDef" SubType="0">
+<TcSmItem xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://www.beckhoff.com/schemas/2012/07/TcSmProject" TcSmVersion="1.0" TcVersion="3.1.4026.17" ClassName="CNcSafTaskDef" SubType="0">
 	<DataTypes>
 		<DataType>
 			<Name GUID="{18071995-0000-0000-0000-000000000035}" TcBaseType="true" HideType="true">UINTARR2</Name>
diff --git a/lcls-twincat-motion/_Config/PLC/Library.xti b/lcls-twincat-motion/_Config/PLC/Library.xti
index 4bb6f1f2..4320e810 100644
--- a/lcls-twincat-motion/_Config/PLC/Library.xti
+++ b/lcls-twincat-motion/_Config/PLC/Library.xti
@@ -983,7 +983,7 @@ External Setpoint Generation:
 		</DataType>
 	</DataTypes>
 	<Project GUID="{E61EF94A-CFE8-4DDF-B7C9-5F7AD2CF9D83}" Name="Library" PrjFilePath="..\..\Library\Library.plcproj" TmcFilePath="..\..\Library\Library.tmc" ReloadTmc="true" AmsPort="851" FileArchiveSettings="#x000e">
-		<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{41370910-2B00-9727-0D9A-BB4963887260}" TmcPath="Library\Library.tmc">
+		<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcHash="{30A73679-D77C-8C3A-4D66-D332CA8B5121}" TmcPath="Library\Library.tmc">
 			<Name>Library Instance</Name>
 			<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
 			<Vars VarGrpType="8" AreaNo="4">
diff --git a/lcls-twincat-motion/lcls-twincat-motion.tsproj b/lcls-twincat-motion/lcls-twincat-motion.tsproj
index f5ff632b..c24b6e11 100644
--- a/lcls-twincat-motion/lcls-twincat-motion.tsproj
+++ b/lcls-twincat-motion/lcls-twincat-motion.tsproj
@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <TcSmProject xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://www.beckhoff.com/schemas/2012/07/TcSmProject" TcSmVersion="1.0" TcVersion="3.1.4026.17" TcVersionFixed="true">
-	<Project ProjectGUID="{29C6E7F0-1B9D-402A-96FB-1C62117E1C0C}" TargetNetId="172.21.148.160.1.1" Target64Bit="true" ShowHideConfigurations="#x3c6">
+	<Project ProjectGUID="{29C6E7F0-1B9D-402A-96FB-1C62117E1C0C}" TargetNetId="172.21.148.154.1.1" Target64Bit="true" ShowHideConfigurations="#x3c6">
 		<System>
 			<Tasks>
 				<Task Id="3" Priority="20" CycleTime="100000" AmsPort="350">
@@ -14,9 +14,5 @@
 		<Plc>
 			<Project File="Library.xti"/>
 		</Plc>
-		<Io/>
 	</Project>
-	<Mappings>
-		<MappingInfo Identifier="{05000010-2001-0850-1000-040300205008}" Id="#x02030010" Watchdog="28000000040000000400000004000000"/>
-	</Mappings>
 </TcSmProject>