FIX: issues relating to "queued" moves in state mover with bMoveOk

lcls-twincat-motion/Library/DUTs/E_LCLSMotionError.TcDUT

@@ -0,0 +1,16 @@
+<?xml version="1.0" encoding="utf-8"?>
+<TcPlcObject Version="1.1.0.1">
+  <DUT Name="E_LCLSMotionError" Id="{ddea4357-8fae-44da-b5b7-bd2ca001f059}">
+    <Declaration><![CDATA[{attribute 'qualified_only'}
+{attribute 'strict'}
+TYPE E_LCLSMotionError :
+(
+    ABORTED := 16#7900,
+    UNSAFE := 16#7901,
+    INVALID := 16#7902,
+    TEST := 16#FFFF
+) UDINT;
+END_TYPE
+]]></Declaration>
+  </DUT>
+</TcPlcObject>

lcls-twincat-motion/Library/Library.plcproj

@@ -105,6 +105,9 @@
     <Compile Include="DUTs\Deprecated\ENUM_StatePMPSStatus.TcDUT">
       <SubType>Code</SubType>
     </Compile>
+    <Compile Include="DUTs\E_LCLSMotionError.TcDUT">
+      <SubType>Code</SubType>
+    </Compile>
     <Compile Include="DUTs\E_MotionFFType.TcDUT">
       <SubType>Code</SubType>
     </Compile>

lcls-twincat-motion/Library/POUs/Motion/FB_MotionRequest.TcPOU

@@ -104,7 +104,7 @@ CASE nState OF
                 E_MotionRequest.ABORT:
                     nState := ERROR;
                     bError := TRUE;
-                    nErrorId := 16#7900;
+                    nErrorId := E_LCLSMotionError.ABORTED;
             END_CASE
         ELSE
             nState := START_MOVE;

lcls-twincat-motion/Library/POUs/Motion/States/FB_PositionStateMove.TcPOU

@@ -91,17 +91,25 @@ VAR_OUTPUT
 END_VAR
 VAR
     fbMotionRequest: FB_MotionRequest;
+    rtExec: R_TRIG;
+    rtReset: R_TRIG;
+    bInnerExec: BOOL;
     bAllowMove: BOOL;
+    nLatchAllowErrorID: UDINT;
 END_VAR
 ]]></Declaration>
     <Implementation>
       <ST><![CDATA[// Veto the move for uninitialized and unsafe states
 bAllowMove := stPositionState.bMoveOk AND stPositionState.bValid AND stPositionState.bUpdated;
 
+rtExec(CLK:=bExecute);
+bInnerExec S= rtExec.Q AND bAllowMove AND NOT bError;
+bInnerExec R= NOT bExecute;
+
 // Do the move
 fbMotionRequest(
     stMotionStage := stMotionStage,
-    bExecute := bExecute AND bAllowMove,
+    bExecute := bInnerExec,
     bReset := bReset,
     enumMotionRequest := enumMotionRequest,
     fPos := stPositionState.fPosition,
@@ -114,15 +122,24 @@ fbMotionRequest(
     bBusy => bBusy,
     bDone => bDone);
 
+rtReset(CLK:=bReset);
+IF rtReset.Q THEN
+    nLatchAllowErrorID := 0;
+END_IF
+
 // Inject custom error if we can't move because of bMoveOk or bValid
-IF bExecute AND NOT bAllowMove THEN
+IF nLatchAllowErrorID <> 0 OR (bExecute AND NOT bAllowMove) THEN
     bError := TRUE;
-    IF stPositionState.bValid THEN
-        nErrorId := 16#7901;
+    IF nLatchAllowErrorID <> 0 THEN
+        nErrorID := nLatchAllowErrorID;
+    ELSIF stPositionState.bValid THEN
+        nErrorID := E_LCLSMotionError.UNSAFE;
     ELSE
-        nErrorId := 16#7902;
+        nErrorID := E_LCLSMotionError.INVALID;
     END_IF
-    sErrorMessage := F_MotionErrorCodeLookup(nErrorId := nErrorID);
+    // Keep error latched until it is cleared, otherwise it can be lost early
+    nLatchAllowErrorID := nErrorID;
+    sErrorMessage := CONCAT(CONCAT(F_MotionErrorCodeLookup(nErrorId := nErrorID), ' for '), stPositionState.sName);
 END_IF
 
 // This can be useful if we're running this FB standalone for some reason

lcls-twincat-motion/Library/POUs/Motion/States/FB_PositionStateND_Core.TcPOU

@@ -69,6 +69,7 @@ fbInput(
     stUserInput:=stEpicsToPlc,
     bMoveBusy:=fbMove.bBusy,
     nStartingState:=fbRead.nPositionIndex,
+    bMoveError:=fbMove.bError,
     nCurrGoal=>nCurrGoal,
     bExecMove=>,
     bResetMove=>,

lcls-twincat-motion/Library/POUs/Motion/States/FB_StatesInputHandler.TcPOU

@@ -19,6 +19,8 @@ VAR_INPUT
     bMoveBusy: BOOL;
     // The starting state number to seed nCurrGoal with
     nStartingState: UINT;
+    // TRUE if we have a move error, to prevent moves
+    bMoveError: BOOL;
 END_VAR
 VAR_OUTPUT
     // The goal index to input to the motion FB. This will be clamped to the range 0..GeneralConstants.MAX_STATES
@@ -84,6 +86,10 @@ CASE nState OF
         END_IF
 END_CASE
 
+IF bMoveError THEN
+    bExecMove := FALSE;
+END_IF
+
 // Help us detect if there is an EPICS put before the next cycle
 stUserInput.nSetValue := 0;
 stUserInput.bReset := FALSE;

lcls-twincat-motion/Library/POUs/Motion/Utils/F_MotionErrorCodeLookup.TcPOU

@@ -29,10 +29,10 @@ END_VAR
     16#4FFF: msg:='Unknown NC error (not in manual)';
 
     // Custom error definitions
-    16#7900: msg:='Aborted move request with active move in progress';
-    16#7901: msg:='Position state unsafe';
-    16#7902: msg:='Position state invalid';
-    16#FFFF: msg:='Fake testing error';
+    E_LCLSMotionError.ABORTED: msg:='Aborted move request with active move in progress';
+    E_LCLSMotionError.UNSAFE: msg:='Position state unsafe';
+    E_LCLSMotionError.INVALID: msg:='Position state invalid';
+    E_LCLSMotionError.TEST: msg:='Fake testing error';
 
     // Fallbacks
     0: msg:='';

lcls-twincat-motion/Library/Tests/FB_PositionStateMove_Test.TcPOU

@@ -21,6 +21,7 @@ VAR
     bOneTestDone: BOOL;
     fTestStartPos: LREAL;
     tonTimer: TON;
+    tonCleanup: TON;
     nIter: DINT;
     bStatesReady: BOOL;
 END_VAR
@@ -85,23 +86,43 @@ TestMove(2, 2, FALSE);
 TestMove(3, 3, TRUE);
 // Test that we cannot move to an invalid state
 TestBadStates(4);
+// Test that we need a new bExecute to start a new move if a move goes from unsafe to safe
+TestMoveOk(5);
 
-IF bOneTestDone THEN
-    bOneTestDone := FALSE;
-    nTestCounter := nTestCounter + 1;
-    fbMove(
-        stMotionStage:=stMotionStage,
-        stPositionState:=stDummyPos,
-        bExecute:=FALSE,
-        bReset:=TRUE,
-    );
-    fbMove(
-        stMotionStage:=stMotionStage,
-        stPositionState:=stDummyPos,
-        bExecute:=FALSE,
-        bReset:=FALSE,
-    );
-    tonTimer(IN:=FALSE);
+// Wait a few cycles for remaining errors to propagate
+tonCleanup(IN:=bOneTestDone, PT:=T#1s);
+IF tonCleanup.Q THEN
+    IF fbMove.bBusy THEN
+        fbMove(
+            stMotionStage:=stMotionStage,
+            stPositionState:=stDummyPos,
+            bExecute:=FALSE,
+            bReset:=FALSE,
+        );
+    ELSIF stMotionStage.bBusy THEN
+        stMotionStage.bExecute := FALSE;
+    ELSIF fbMove.bError THEN
+        fbMove(
+            stMotionStage:=stMotionStage,
+            stPositionState:=stDummyPos,
+            bExecute:=FALSE,
+            bReset:=TRUE,
+        );
+    ELSIF stMotionStage.bError THEN
+       stMotionStage.bReset := TRUE;
+    ELSE
+        fbMove(
+            stMotionStage:=stMotionStage,
+            stPositionState:=stDummyPos,
+            bExecute:=FALSE,
+            bReset:=FALSE,
+        );
+        stMotionStage.bReset := FALSE;
+
+        bOneTestDone := FALSE;
+        nTestCounter := nTestCounter + 1;
+        tonTimer(IN:=FALSE);
+    END_IF
 END_IF
 // Use this timer to time out any tests that stall
 tonTimer(
@@ -118,7 +139,7 @@ END_VAR
 ]]></Declaration>
       <Implementation>
         <ST><![CDATA[TEST(CONCAT('TestInvalid', UINT_TO_STRING(nTestIndex)));
-IF nTestCounter <> nTestIndex THEN
+IF nTestCounter <> nTestIndex OR bOneTestDone THEN
     RETURN;
 END_IF
 
@@ -201,11 +222,12 @@ END_VAR
 VAR_INST
     bLocalInit: BOOL;
     bInterruptStarted: BOOL;
+    tonBusy: TON;
 END_VAR
 ]]></Declaration>
       <Implementation>
         <ST><![CDATA[TEST(CONCAT('TestMove', UINT_TO_STRING(nTestIndex)));
-IF nTestCounter <> nTestIndex THEN
+IF nTestCounter <> nTestIndex OR bOneTestDone THEN
     RETURN;
 END_IF
 
@@ -222,7 +244,10 @@ IF NOT bLocalInit THEN
     bLocalInit := TRUE;
 END_IF
 
-bInterruptStarted S= bInterrupt AND stMotionStage.bBusy;
+// Simulate waiting a moment before stopping
+tonBusy(IN:=stMotionStage.bBusy, PT:=T#100ms);
+bInterruptStarted S= bInterrupt AND tonBusy.Q;
+
 fbMove(
     stMotionStage:=stMotionstage,
     stPositionState:=astPositionState[nStateIndex],
@@ -268,5 +293,145 @@ END_IF
 ]]></ST>
       </Implementation>
     </Method>
+    <Method Name="TestMoveOk" Id="{0a0e07ec-6039-465c-88e5-a08bb2253266}">
+      <Declaration><![CDATA[METHOD TestMoveOk
+VAR_INPUT
+    nTestIndex: UINT;
+END_VAR
+VAR_INST
+    nStep: UINT;
+    fStartPos: LREAL;
+    stState: ST_PositionState := (
+        sName:='GOAL',
+        fPosition:=135,
+        fDelta:=0.5,
+        fVelocity:=1,
+        bValid:=TRUE,
+        bMoveOk:=FALSE,
+        bUpdated:=TRUE
+    );
+    tonInternal: TON;
+    bLocalExec: BOOL;
+    bLocalReset: BOOL;
+END_VAR
+VAR CONSTANT
+    END: UINT := 999;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[TEST('TestMoveOk');
+IF nTestCounter <> nTestIndex OR bOneTestDone THEN
+    RETURN;
+END_IF
+
+CASE nStep OF
+    // Initial setup and checks
+    0:
+        fStartPos := stMotionStage.stAxisStatus.fActPosition;
+        AssertFalse(
+            fbMove.bError,
+            'Started with a fbMove error',
+        );
+        AssertFalse(
+            stMotionStage.bError,
+            'Started with a motor error',
+        );
+        bLocalExec := FALSE;
+        bLocalReset := FALSE;
+        nStep := nStep + 1;
+    // Move to a state that is bad
+    1:
+        stState.bMoveOk := FALSE;
+        bLocalExec := TRUE;
+        nStep := nStep + 1;
+    // The move should have failed with an error on the FB that never propagated to the motor itself- we shouldn't have attempted the move at all
+    2:
+        AssertTrue(
+            fbMove.bError,
+            'Did not have the expected move not ok error',
+        );
+        AssertFalse(
+            stMotionStage.bError,
+            'Had a motion error, but we never should have asked for a move?',
+        );
+        nStep := nStep + 1;
+    // Make the state no longer bad, and wait a bit
+    3:
+        stState.bMoveOk := TRUE;
+        tonInternal(IN:=TRUE, PT:=T#500ms);
+        IF tonInternal.Q THEN
+            tonInternal(IN:=FALSE);
+            nStep := nStep + 1;
+        END_IF
+    // There should be no movement still
+    4:
+        AssertEquals_LREAL(
+            Expected:=fStartPos,
+            Actual:=stMotionStage.stAxisStatus.fActPosition,
+            Delta:=0.1,
+            Message:='Motor moved without bReset or a new bExecute!',
+        );
+        // Reset the error (rising edge)
+        bLocalReset := TRUE;
+        nStep := nStep + 1;
+    // After we reset the error, wait a bit
+    5:
+        // Drop for rising edges later
+        bLocalReset := FALSE;
+        tonInternal(IN:=TRUE, PT:=T#500ms);
+        IF tonInternal.Q THEN
+            tonInternal(IN:=FALSE);
+            nStep := nStep + 1;
+        END_IF
+    // There should be no error, and STILL be no movement
+    6:
+        AssertFalse(
+            fbMove.bError,
+            'The error should be reset',
+        );
+        AssertEquals_LREAL(
+            Expected:=fStartPos,
+            Actual:=stMotionStage.stAxisStatus.fActPosition,
+            Delta:=0.1,
+            Message:='Motor moved without a new bExecute!',
+        );
+        // Set bExecute to FALSE for an upcoming rising edge. It should be TRUE at this point.
+        bLocalExec := FALSE;
+        nStep := nStep + 1;
+    // We should be able to start a move via bExecute now
+    7:
+        stState.fVelocity := 2000;
+        stState.fAccel := 15000;
+        stState.fDecel := 15000;
+        bLocalExec := TRUE;
+        IF fbMove.bAtState AND stMotionStage.bDone THEN
+            nStep := END;
+        END_IF
+END_CASE
+
+// Run fbMove exactly once every cycle no matter what
+fbMove(
+    stMotionStage:=stMotionStage,
+    stPositionState:=stState,
+    bExecute:=bLocalExec,
+    bReset:=bLocalReset,
+);
+
+IF nStep = END OR tonTimer.Q THEN
+    AssertFalse(
+        tonTimer.Q,
+        'Test timed out',
+    );
+    // Check that we've reached the goal
+    AssertEquals_LREAL(
+        Expected:=stState.fPosition,
+        Actual:=stMotionStage.stAxisStatus.fActPosition,
+        Delta:=0.1,
+        Message:='Motor did not reach the goal.',
+    );
+    bOneTestDone := TRUE;
+    TEST_FINISHED();
+END_IF]]></ST>
+      </Implementation>
+    </Method>
   </POU>
 </TcPlcObject>

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" TmcPath="Library\Library.tmc" TmcHash="{1FCA17A7-AF74-AD80-7B07-555636D3E07F}">
+		<Instance Id="#x08502000" TcSmClass="TComPlcObjDef" KeepUnrestoredLinks="2" TmcPath="Library\Library.tmc" TmcHash="{0C997D26-FEC1-46B4-2438-632FFA910317}">
 			<Name>Library Instance</Name>
 			<CLSID ClassFactory="TcPlc30">{08500001-0000-0000-F000-000000000064}</CLSID>
 			<Vars VarGrpType="1">