From 1079229ffb7948f452ddd2b3b5515f9300a066db Mon Sep 17 00:00:00 2001
From: Simon-Cooper <39404307+Simon-Cooper@users.noreply.github.com>
Date: Fri, 7 Feb 2020 10:17:46 +0000
Subject: [PATCH] ALLOW LOCAL SIMULATIONS TO BYBASS UPS CHECK

Add CHECK_UPS action to house monitoring of the UPS global status
Modify RESTORE_POSITIONS state machine to remove numerical enumeration
usage
Add device identity functionblock to check whether local simulation or
CX
Add POSITION RECOVERY action to check device type and determine whether
to run
position restore and ups checking
Add solution/_Config/ folder to gitignore to remove NC and IO
---
 .gitignore                              |   1 +
 solution/solution.tsproj                | 129 ++++++++----------------
 solution/tc_project_app/POUs/MAIN.TcPOU |  73 +++++++++-----
 3 files changed, 91 insertions(+), 112 deletions(-)

diff --git a/.gitignore b/.gitignore
index 8f8c117..cab1dc8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -17,3 +17,4 @@ logs.0*
 solution/TrialLicense.tclrs
 tools/linux/ADS/
 tools/linux/getADSState/getADSState.bin
+_Config/
diff --git a/solution/solution.tsproj b/solution/solution.tsproj
index f74cba4..5dd70ed 100644
--- a/solution/solution.tsproj
+++ b/solution/solution.tsproj
@@ -1,8 +1,8 @@
 <?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.4023.119">
+<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.4024.4">
 	<DataTypes>
 		<DataType>
-			<Name GUID="{4C3FC5AC-D5AA-44C6-AC5A-159774BA0F6D}" Namespace="MC" TcBaseType="true" HideType="true" IecDeclaration="DWORD;">NCTOPLC_AXIS_REF_STATE</Name>
+			<Name GUID="{CBC83B73-B816-4597-A9E5-2B03263CA131}" Namespace="MC" TcBaseType="true" HideType="true" IecDeclaration="DWORD;">NCTOPLC_AXIS_REF_STATE</Name>
 			<BitSize>32</BitSize>
 			<SubItem>
 				<Name>Operational</Name>
@@ -112,6 +112,12 @@
 				<BitSize>1</BitSize>
 				<BitOffs>17</BitOffs>
 			</SubItem>
+			<SubItem>
+				<Name>IsDriveLimitActive</Name>
+				<Type GUID="{18071995-0000-0000-0000-000000000010}">BIT</Type>
+				<BitSize>1</BitSize>
+				<BitOffs>18</BitOffs>
+			</SubItem>
 			<SubItem>
 				<Name>ContinuousMotion</Name>
 				<Type GUID="{18071995-0000-0000-0000-000000000010}">BIT</Type>
@@ -199,6 +205,11 @@
 			<Format Name="IEC">
 				<Printf>16#%08X</Printf>
 			</Format>
+			<Relations>
+				<Relation Priority="100">
+					<Type>{4C3FC5AC-D5AA-44C6-AC5A-159774BA0F6D}</Type>
+				</Relation>
+			</Relations>
 		</DataType>
 		<DataType>
 			<Name GUID="{6EF49753-C72C-4F50-AA44-3C7498E76CFE}" Namespace="MC" TcBaseType="true" HideType="true" IecDeclaration="DWORD;">NCTOPLC_AXIS_REF_OPMODE</Name>
@@ -394,11 +405,11 @@
 			</ArrayInfo>
 		</DataType>
 		<DataType>
-			<Name GUID="{6A65C767-34E5-42BF-AD87-E1A503EAC7BE}" Namespace="MC" TcBaseType="true">NCTOPLC_AXIS_REF</Name>
+			<Name GUID="{72F5AAAA-16DF-4ED3-8367-F6C8C3ADAE99}" Namespace="MC" TcBaseType="true">NCTOPLC_AXIS_REF</Name>
 			<BitSize>2048</BitSize>
 			<SubItem>
 				<Name>StateDWord</Name>
-				<Type GUID="{4C3FC5AC-D5AA-44C6-AC5A-159774BA0F6D}" Namespace="MC">NCTOPLC_AXIS_REF_STATE</Type>
+				<Type GUID="{CBC83B73-B816-4597-A9E5-2B03263CA131}" Namespace="MC">NCTOPLC_AXIS_REF_STATE</Type>
 				<BitSize>32</BitSize>
 				<BitOffs>0</BitOffs>
 			</SubItem>
@@ -646,6 +657,18 @@ External Setpoint Generation:
 				<BitSize>64</BitSize>
 				<BitOffs>1600</BitOffs>
 			</SubItem>
+			<SubItem>
+				<Name>AbsPhasingPos</Name>
+				<Type GUID="{18071995-0000-0000-0000-00000000000E}">LREAL</Type>
+				<BitSize>64</BitSize>
+				<BitOffs>1664</BitOffs>
+			</SubItem>
+			<SubItem>
+				<Name>TorqueOffset</Name>
+				<Type GUID="{18071995-0000-0000-0000-00000000000E}">LREAL</Type>
+				<BitSize>64</BitSize>
+				<BitOffs>1728</BitOffs>
+			</SubItem>
 			<SubItem>
 				<Name>ActPosWithoutPosCorrection</Name>
 				<Type GUID="{18071995-0000-0000-0000-00000000000E}">LREAL</Type>
@@ -692,6 +715,9 @@ External Setpoint Generation:
 				<Relation Priority="100">
 					<Type GUID="{8CDE0C45-AB9D-42DB-BC94-1CF7521AB268}"></Type>
 				</Relation>
+				<Relation Priority="100">
+					<Type GUID="{6A65C767-34E5-42BF-AD87-E1A503EAC7BE}"></Type>
+				</Relation>
 			</Relations>
 		</DataType>
 		<DataType>
@@ -873,7 +899,7 @@ External Setpoint Generation:
 			</Relations>
 		</DataType>
 	</DataTypes>
-	<Project ProjectGUID="{9CF97348-B9D3-4938-B1F2-5F0B0B6AA66A}" TargetNetId="5.54.104.144.1.1" Target64Bit="true" ShowHideConfigurations="#x106">
+	<Project ProjectGUID="{9CF97348-B9D3-4938-B1F2-5F0B0B6AA66A}" Target64Bit="true" ShowHideConfigurations="#x106">
 		<System>
 			<Licenses>
 				<Target>
@@ -889,6 +915,9 @@ External Setpoint Generation:
 				</Task>
 			</Tasks>
 		</System>
+		<Motion>
+			<NC File="NC.xti"/>
+		</Motion>
 		<Plc>
 			<Project GUID="{F935F1DE-0753-4702-B418-1DC0ED040A4D}" Name="tc_epicscommodule" PrjFilePath="tc_epicscommodule\tc_epicscommodule.plcproj" TmcFilePath="tc_epicscommodule\tc_epicscommodule.tmc" ReloadTmc="true" AmsPort="851" FileArchiveSettings="#x000e" SymbolicMapping="true">
 				<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="tc_epicscommodule\tc_epicscommodule.tmc">
@@ -927,46 +956,7 @@ External Setpoint Generation:
 						</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>
+							<Type GUID="{72F5AAAA-16DF-4ED3-8367-F6C8C3ADAE99}" Namespace="MC">NCTOPLC_AXIS_REF</Type>
 						</Var>
 						<Var>
 							<Name>GVL.axes[2].inputs.bLimitFwd</Name>
@@ -990,46 +980,7 @@ External Setpoint Generation:
 						</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>
+							<Type GUID="{72F5AAAA-16DF-4ED3-8367-F6C8C3ADAE99}" Namespace="MC">NCTOPLC_AXIS_REF</Type>
 						</Var>
 					</Vars>
 					<Vars VarGrpType="2">
@@ -1055,4 +1006,12 @@ External Setpoint Generation:
 			</Project>
 		</Plc>
 	</Project>
+	<Mappings>
+		<OwnerA Name="TIPC^tc_project_app^tc_project_app Instance">
+			<OwnerB Name="TINC^NC-Task 1 SAF^Axes^Axis 1">
+				<Link VarA="PlcTask Inputs^GVL.axes[1].Axis.NcToPlc" VarB="Outputs^ToPlc"/>
+				<Link VarA="PlcTask Outputs^GVL.axes[1].Axis.PlcToNc" VarB="Inputs^FromPlc"/>
+			</OwnerB>
+		</OwnerA>
+	</Mappings>
 </TcSmProject>
diff --git a/solution/tc_project_app/POUs/MAIN.TcPOU b/solution/tc_project_app/POUs/MAIN.TcPOU
index f81cabd..5eebc02 100644
--- a/solution/tc_project_app/POUs/MAIN.TcPOU
+++ b/solution/tc_project_app/POUs/MAIN.TcPOU
@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="utf-8"?>
-<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.0">
+<TcPlcObject Version="1.1.0.1" ProductVersion="3.1.4024.2">
   <POU Name="MAIN" Id="{33eb6f49-7781-4211-a70b-87ada6d80cb7}" SpecialFunc="None">
     <Declaration><![CDATA[PROGRAM MAIN
 VAR
@@ -48,6 +48,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 +56,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 +79,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 +126,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 +143,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.
@@ -176,14 +190,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 +213,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 +231,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 +246,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 +259,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 +273,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="444" Count="0" />
       <LineId Id="134" Count="0" />
       <LineId Id="81" Count="1" />
     </LineIds>
@@ -271,6 +282,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 +302,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" />