diff --git a/.gitignore b/.gitignore
index 8f8c117..63a08bd 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,5 @@ logs.0*
 solution/TrialLicense.tclrs
 tools/linux/ADS/
 tools/linux/getADSState/getADSState.bin
+_Config/NC/Axes
+_Config/IO
diff --git a/solution/_Config/NC/NC.xti b/solution/_Config/NC/NC.xti
index 2c8f8d7..759c7a3 100644
--- a/solution/_Config/NC/NC.xti
+++ b/solution/_Config/NC/NC.xti
@@ -16,6 +16,5 @@
 		<SvbTask Priority="8" CycleTime="100000" AmsPort="511">
 			<Name>NC-Task 1 SVB</Name>
 		</SvbTask>
-		<Axis File="tcUNIT_virtual_axis.xti" Id="1"/>
 	</NC>
 </TcSmItem>
diff --git a/solution/solution.tsproj b/solution/solution.tsproj
index 8c3ec91..bcc972e 100644
--- a/solution/solution.tsproj
+++ b/solution/solution.tsproj
@@ -906,135 +906,6 @@ External Setpoint Generation:
 				<Instance Id="#x08502040" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="tc_project_app\tc_project_app.tmc">
 					<Name>tc_project_app Instance</Name>
 					<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
-					<Vars VarGrpType="1">
-						<Name>PlcTask Inputs</Name>
-						<Var>
-							<Name>GVL.axes[1].inputs.bLimitFwd</Name>
-							<Comment><![CDATA[////Axis Inputs///////]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[1].inputs.bLimitBwd</Name>
-							<Comment><![CDATA[Backward limit switch]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[1].inputs.bHomeSensor</Name>
-							<Comment><![CDATA[Reference siganl for homing]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[1].inputs.bEncLAtch</Name>
-							<Comment><![CDATA[Reference index signal from inc. enconder]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[1].Axis.NcToPlc</Name>
-							<Type GUID="{6A65C767-34E5-42BF-AD87-E1A503EAC7BE}" Namespace="MC">NCTOPLC_AXIS_REF</Type>
-							<SubVar>
-								<Name>AxisState</Name>
-								<Comment><![CDATA[Present State Of The Axis Movement (continuous axis):
-0  = INACTIVE:		axis has no job
-1  = RUNNING:		axis is executing a motion job
-2  = OVERRIDE_ZERO:	axis is executing a job but override is zero
-3  = PHASE_VELOCONST:	axis is moving at constant velocity
-4  = PHASE_ACCPOS:	axis is accelerating
-5  = PHASE_ACCNEG:	axis is decelerating
-Slaves only:
-11 = PREPHASE:		slave axis is in a motion pre-phase
-12 = SYNCHRONIZING:	slave axis is synchronizing
-13 = SYNCHRONOUS:	slave axis is moving synchronously
-External Setpoint Generation:
-41 = EXTSETGEN_MODE1:	external setpoint generation active
-42 = EXTSETGEN_MODE2:	internal and external setpoint gen. active
-]]></Comment>
-							</SubVar>
-							<SubVar>
-								<Name>HomingState</Name>
-								<Comment><![CDATA[Axis Homing Status:
-0: idle
-1: start homing
-2: searching home switch
-3: stopping on home switch
-4: moving off home switch
-5: searching sync pulse
-6: stopping after homing
-]]></Comment>
-							</SubVar>
-							<SubVar>
-								<Name>CoupleState</Name>
-								<Comment><![CDATA[Axis Coupling Status:
-0: axis is a single axis (not coupled)
-1: axis is a master axis
-2: axis is master and slave
-3: axis is a slave axis
-]]></Comment>
-							</SubVar>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].inputs.bLimitFwd</Name>
-							<Comment><![CDATA[////Axis Inputs///////]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].inputs.bLimitBwd</Name>
-							<Comment><![CDATA[Backward limit switch]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].inputs.bHomeSensor</Name>
-							<Comment><![CDATA[Reference siganl for homing]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].inputs.bEncLAtch</Name>
-							<Comment><![CDATA[Reference index signal from inc. enconder]]></Comment>
-							<Type>BOOL</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].Axis.NcToPlc</Name>
-							<Type GUID="{6A65C767-34E5-42BF-AD87-E1A503EAC7BE}" Namespace="MC">NCTOPLC_AXIS_REF</Type>
-							<SubVar>
-								<Name>AxisState</Name>
-								<Comment><![CDATA[Present State Of The Axis Movement (continuous axis):
-0  = INACTIVE:		axis has no job
-1  = RUNNING:		axis is executing a motion job
-2  = OVERRIDE_ZERO:	axis is executing a job but override is zero
-3  = PHASE_VELOCONST:	axis is moving at constant velocity
-4  = PHASE_ACCPOS:	axis is accelerating
-5  = PHASE_ACCNEG:	axis is decelerating
-Slaves only:
-11 = PREPHASE:		slave axis is in a motion pre-phase
-12 = SYNCHRONIZING:	slave axis is synchronizing
-13 = SYNCHRONOUS:	slave axis is moving synchronously
-External Setpoint Generation:
-41 = EXTSETGEN_MODE1:	external setpoint generation active
-42 = EXTSETGEN_MODE2:	internal and external setpoint gen. active
-]]></Comment>
-							</SubVar>
-							<SubVar>
-								<Name>HomingState</Name>
-								<Comment><![CDATA[Axis Homing Status:
-0: idle
-1: start homing
-2: searching home switch
-3: stopping on home switch
-4: moving off home switch
-5: searching sync pulse
-6: stopping after homing
-]]></Comment>
-							</SubVar>
-							<SubVar>
-								<Name>CoupleState</Name>
-								<Comment><![CDATA[Axis Coupling Status:
-0: axis is a single axis (not coupled)
-1: axis is a master axis
-2: axis is master and slave
-3: axis is a slave axis
-]]></Comment>
-							</SubVar>
-						</Var>
-					</Vars>
 					<Vars VarGrpType="2">
 						<Name>PlcTask Outputs</Name>
 						<Var>
@@ -1042,14 +913,6 @@ External Setpoint Generation:
 							<Comment><![CDATA[*****Outputs: Power for Limit switches and Home Sensors (every 4th output)*******]]></Comment>
 							<Type>BOOL</Type>
 						</Var>
-						<Var>
-							<Name>GVL.axes[1].Axis.PlcToNc</Name>
-							<Type GUID="{63A84524-72E3-41C8-BEAB-4CCE44690A13}" Namespace="MC">PLCTONC_AXIS_REF</Type>
-						</Var>
-						<Var>
-							<Name>GVL.axes[2].Axis.PlcToNc</Name>
-							<Type GUID="{63A84524-72E3-41C8-BEAB-4CCE44690A13}" Namespace="MC">PLCTONC_AXIS_REF</Type>
-						</Var>
 					</Vars>
 					<TaskPouOids>
 						<TaskPouOid Prio="20" OTCID="#x08502041"/>
diff --git a/solution/tc_project_app/GVLs/GVL_APP.TcGVL b/solution/tc_project_app/GVLs/GVL_APP.TcGVL
index 9e800ab..e125049 100644
--- a/solution/tc_project_app/GVLs/GVL_APP.TcGVL
+++ b/solution/tc_project_app/GVLs/GVL_APP.TcGVL
@@ -7,7 +7,7 @@ VAR_GLOBAL
 END_VAR
 
 VAR_GLOBAL CONSTANT
-    axisNum :   UINT:=2;
+    axisNum :   UINT:=0;
     //axisCoupleMax :   UINT:=4;
 END_VAR]]></Declaration>
   </GVL>
diff --git a/solution/tc_project_app/POUs/MAIN.TcPOU b/solution/tc_project_app/POUs/MAIN.TcPOU
index f81cabd..2621b5e 100644
--- a/solution/tc_project_app/POUs/MAIN.TcPOU
+++ b/solution/tc_project_app/POUs/MAIN.TcPOU
@@ -6,6 +6,7 @@ VAR
     sVersion: STRING:='1.0.0';
     i : UINT; //index variable for AXES()
     aFbAxes: ARRAY [1..gvl_app.axisNum] OF FB_Axis;
+
     hmiAxisSelection    :   INT:=1;     //Not possible to use local hmi variables for array indexes
 
 (******Outputs: Power for Limit switches and Home Sensors (every 4th output)********)
@@ -48,6 +49,7 @@ VAR
     iRetry                 : INT;
     fbReadEncRefSys        : ARRAY [1..gvl_app.axisNum] OF MC_ReadParameter;
     fbRestorePosition      : ARRAY [1..GVL_app.axisNum] OF MC_SetPosition;
+    fbGetDeviceIdentification : FB_GetDeviceIdentification;
 
 END_VAR
 
@@ -55,7 +57,22 @@ VAR PERSISTENT
     bRestoreOnStartup : BOOL;
 END_VAR]]></Declaration>
     <Implementation>
-      <ST><![CDATA[fbUPS(eUpsMode := eUpsMode); (* call UPS-FB instance in first lines of the fastest PLC Task *)
+      <ST><![CDATA[POSITION_RECOVERY();
+PROG();
+AXES();
+ERROR();]]></ST>
+    </Implementation>
+    <Folder Name="POSITION_RECOVERY" Id="{3561f6ef-e145-4ed3-9839-f17334bd2d97}" />
+    <Action Name="AXES" Id="{7eb32732-9b53-4934-8cd9-20ba971dd8ff}">
+      <Implementation>
+        <ST><![CDATA[FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
+    aFbAxes[GVL.iAxis](stAxisStruct:=gvl.axes[GVL.iAxis]);
+END_FOR]]></ST>
+      </Implementation>
+    </Action>
+    <Action Name="CHECK_UPS" Id="{f0f28f50-53b8-4f73-b0f5-6d7ce4c1636f}" FolderPath="POSITION_RECOVERY\">
+      <Implementation>
+        <ST><![CDATA[fbUPS(eUpsMode := eUpsMode); (* call UPS-FB instance in first lines of the fastest PLC Task *)
 
 IF eGlobalSUpsState = eSUPS_PowerFailure THEN
     (* first cycle of powerfailure *)
@@ -63,22 +80,11 @@ IF eGlobalSUpsState = eSUPS_PowerFailure THEN
     bRestoreOnStartup:=TRUE;
     STORE_PERSISTENT();
     RETURN;
-ELSIF eGlobalSUpsState > eSUPS_PowerFailure THEN
+ELSIF eGlobalSUpsState <> eSUPS_PowerOK THEN
     (* next cycles of powerfailure *)
     (* skip regular code execution for the remaining cycles of the powerfailure/writing of persistent data/quick shutdown  ... *)
     RETURN;
-END_IF
-
-RESTORE_POSITIONS();
-PROG();
-AXES();
-ERROR();]]></ST>
-    </Implementation>
-    <Action Name="AXES" Id="{7eb32732-9b53-4934-8cd9-20ba971dd8ff}">
-      <Implementation>
-        <ST><![CDATA[FOR GVL.iAxis:=1 TO gvl_app.axisNum DO
-    aFbAxes[GVL.iAxis](stAxisStruct:=gvl.axes[GVL.iAxis]);
-END_FOR]]></ST>
+END_IF]]></ST>
       </Implementation>
     </Action>
     <Action Name="ERROR" Id="{35f2cf38-f81e-4aa3-9534-be5fb417817d}">
@@ -121,6 +127,15 @@ fbEL1808(
 ]]></ST>
       </Implementation>
     </Action>
+    <Action Name="POSITION_RECOVERY" Id="{28e203b7-aea5-42d0-980d-12a6841f9d22}" FolderPath="POSITION_RECOVERY\">
+      <Implementation>
+        <ST><![CDATA[fbGetDeviceIdentification(bExecute:=TRUE);
+IF (fbGetDeviceIdentification.stDevIdent.strHardwareSerialNo <> '0') THEN
+    CHECK_UPS();
+    RESTORE_POSITIONS();
+END_IF]]></ST>
+      </Implementation>
+    </Action>
     <Action Name="PROG" Id="{5d03ebbb-2a47-4890-ad6d-e82daf72dc51}">
       <Implementation>
         <ST><![CDATA[//
@@ -129,7 +144,7 @@ fbEL1808(
 //]]></ST>
       </Implementation>
     </Action>
-    <Action Name="RESTORE_POSITIONS" Id="{0c7ee537-7bd9-4833-b428-c17cbb57e893}">
+    <Action Name="RESTORE_POSITIONS" Id="{0c7ee537-7bd9-4833-b428-c17cbb57e893}" FolderPath="POSITION_RECOVERY\">
       <Implementation>
         <ST><![CDATA[///#########################################################
 // This ACT will restore the position of an incremental axis on startup with the act position it read before losing power.
@@ -144,7 +159,7 @@ fbEL1808(
 // Note from Beckhoff: "A maximum of 1 MB persistent data can be reliably saved over the entire service life."
 ///#########################################################
 
-IF bRestoreOnStartup THEN
+IF bRestoreOnStartup AND eGlobalSUpsState = eSUPS_PowerOK THEN
     bRestoreOnStartup:=FALSE;
     bRestoreExecute:=TRUE;
 END_IF
@@ -176,14 +191,14 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
         ColdStart:
         // First cycle of the PLC, do nothing just give one cycle for variables to initialise
             IF NOT bPositionRestoreDone THEN
-                eStartUp:=eStartUp+1;
+                eStartUp:= ReadAxisFeedbackType;
                 iRetry:=0;
             END_IF
 
         ReadAxisFeedbackType:
         // Exectute the function blocks to read the encoder reference system (0=inc OR 1=ABS)
             bExecuteReadEncRefSys:=TRUE;
-            eStartUp:=eStartUp+1;
+            eStartUp:=CheckReadDone;
 
         CheckReadDone:
         // Check the encoder reference system has been read for all axis -> if busy then continue with PLC cycle and check again next time.
@@ -199,14 +214,14 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
                         // .valid=FALSE and .busy=FALSE which indicateds the FB probably hasn't started and thus needs to see a rising edge.
                         // Set execute to low, Exit MAIN.STARTUP and go back a step in the CASE statement.
                         bExecuteReadEncRefSys:=FALSE;
-                        eStartUp:=eStartUp-1;
+                        eStartUp:=ReadAxisFeedbackType;
                         iRetry:=iRetry+1; // counter used for troubleshooting to see how many cycles it takes before fbReadEncRefSys function blocks are read correctly
                         RETURN;
                     END_IF
                 END_IF
             END_FOR
             // If the code gets here all axes either have .valid=TRUE for all axes
-            eStartUp:=eStartUp+1;
+            eStartUp:= ExecuteRestore;
 
         ExecuteRestore:
         // Execute position restore by setting fbRestorePosition.execute = TRUE
@@ -217,7 +232,7 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
                     END_IF
                 END_IF
             END_FOR
-            eStartUp:=eStartUp+1;
+            eStartUp:= CheckRestore;
 
         CheckRestore:
         // Check the set position fbs are finished
@@ -232,7 +247,7 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
                     END_IF
                 END_IF
             END_FOR
-            eStartUp:=eStartUp+1;
+            eStartUp:= FinishRestore;
 
         FinishRestore:
         // Remove execute = TRUE for fbRestorePosition
@@ -245,7 +260,7 @@ IF bRestoreExecute AND NOT bPositionRestoreDone THEN
 END_IF]]></ST>
       </Implementation>
     </Action>
-    <Action Name="STORE_PERSISTENT" Id="{cb5c9254-2e5f-47b1-9baa-10e728a961b0}">
+    <Action Name="STORE_PERSISTENT" Id="{cb5c9254-2e5f-47b1-9baa-10e728a961b0}" FolderPath="POSITION_RECOVERY\">
       <Implementation>
         <ST><![CDATA[FOR i:=1 TO gvl_app.axisNum DO
     axesPersistent[i].iPositionAtShutdown:=gvl.axes[i].Axis.NcToPlc.ActPos;
@@ -259,10 +274,7 @@ END_FOR]]></ST>
       </Implementation>
     </Action>
     <LineIds Name="MAIN">
-      <LineId Id="376" Count="11" />
-      <LineId Id="2" Count="0" />
-      <LineId Id="189" Count="0" />
-      <LineId Id="135" Count="0" />
+      <LineId Id="505" Count="0" />
       <LineId Id="134" Count="0" />
       <LineId Id="81" Count="1" />
     </LineIds>
@@ -271,6 +283,10 @@ END_FOR]]></ST>
       <LineId Id="4" Count="0" />
       <LineId Id="10" Count="0" />
     </LineIds>
+    <LineIds Name="MAIN.CHECK_UPS">
+      <LineId Id="2" Count="11" />
+      <LineId Id="1" Count="0" />
+    </LineIds>
     <LineIds Name="MAIN.ERROR">
       <LineId Id="31" Count="0" />
       <LineId Id="10" Count="1" />
@@ -287,6 +303,10 @@ END_FOR]]></ST>
       <LineId Id="14" Count="0" />
       <LineId Id="33" Count="0" />
     </LineIds>
+    <LineIds Name="MAIN.POSITION_RECOVERY">
+      <LineId Id="2" Count="3" />
+      <LineId Id="1" Count="0" />
+    </LineIds>
     <LineIds Name="MAIN.PROG">
       <LineId Id="2" Count="0" />
       <LineId Id="1" Count="0" />