Sequencing ND state moves

.gitmodules

@@ -1,3 +1,3 @@
 [submodule "lcls-twincat-motion/Library/tc_mca_std_lib"]
 	path = lcls-twincat-motion/Library/tc_mca_std_lib
-	url = ../tc_mca_std_lib.git
+	url = [email protected]:pcdshub/tc_mca_std_lib.git

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

@@ -68,6 +68,24 @@
 
     // We give this a state name and it is used to load parameters from the pmps database.
     stPMPS: ST_DbStateParams;
+
+	// Set this to TRUE to enable use of the iSequence value to sequence moves that incorporate this state.
+    {attribute 'pytmc' := '
+        pv: SEQUENCE_MOVES
+        io: i
+        field: DESC TRUE if this state represents a state intended to be moved in sequence with other axes
+    '}
+	bSequenceMoves: BOOl;
+
+	// Set this value to the sequence number that you want this state to have relative to other axes.
+	// E.g. if this state has iSequence = 2 and it is being moved with a state with iSequence = 1, then
+	// the iSequence = 1 state will be moved first and this state will only be moved after.
+    {attribute 'pytmc' := '
+        pv: SEQUENCE_ORDER
+        io: i
+        field: DESC Relative sequence order for the axis movement corresponding to this state.
+    '}
+	iSequenceOrder: UINT := 1;
 END_STRUCT
 END_TYPE]]></Declaration>
   </DUT>

lcls-twincat-motion/Library/POUs/Motion/PMPS/FB_StatePMPSEnables.TcPOU

@@ -47,6 +47,7 @@ VAR
     fLowerPos: LREAL;
     fUpperPos: LREAL;
     ffNoGoal: FB_FastFault;
+    bLockBoundsOn: BOOL;
 END_VAR]]></Declaration>
     <Implementation>
       <ST><![CDATA[
@@ -98,13 +99,21 @@ mc_power(
 IF nGoalStateIndex > 0 AND nGoalStateIndex <= GeneralConstants.MAX_STATES THEN
     IF astPositionState[nGoalStateIndex].bValid AND astPositionState[nGoalStateIndex].bUpdated THEN
         bValidGoal := TRUE;
+        bLockBoundsOn := TRUE;
         fLowerPos := astPositionState[nGoalStateIndex].fPosition - ABS(astPositionState[nGoalStateIndex].fDelta);
         fUpperPos := astPositionState[nGoalStateIndex].fPosition + ABS(astPositionState[nGoalStateIndex].fDelta);
     ELSE
         bValidGoal := FALSE;
     END_IF
 ELSE
     bValidGoal := FALSE;
+END_IF
+// Only set the bounds lock false when the function block is disabled.
+// E.g.: either position limits are explicitly disabled or we enter maintenance mode.
+// Once bounds are disabled once, we wait until we have a new valid goal position state
+// to re-enable bounds.
+IF NOT bEnable THEN
+    bLockBoundsOn := FALSE;
 END_IF]]></ST>
       </Implementation>
     </Action>
@@ -129,11 +138,12 @@ ffNoGoal(
     This action sets bForwardEnable and bBackwardEnable based on
     the current position and the calculated bounds.
 *)
-IF bValidGoal AND bEnable THEN
+IF bLockBoundsOn AND bEnable THEN
     bForwardEnabled := stMotionStage.stAxisStatus.fActPosition < fUpperPos;
     bBackwardEnabled := stMotionStage.stAxisStatus.fActPosition > fLowerPos;
 ELSE
     // Either invalid state with a fault or FB not enabled
+    // Bounds are locked active when a valid goal state is commanded
     bForwardEnabled := TRUE;
     bBackwardEnabled := TRUE;
 END_IF

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

@@ -5,7 +5,7 @@
 FUNCTION_BLOCK FB_PositionStateMoveND
 (*
     This function block coordinates multidimensional state moves for groups of motors.
-    It is a building block not meant for use outside of lcls-twintcat-motion.
+    It is a building block not meant for use outside of lcls-twincat-motion.
 
     Use FB_PositionState1D, FB_PositionState2D, ... etc. instead
 *)
@@ -50,6 +50,9 @@ VAR
     bMotorCountError: BOOL;
     nLowerBound: DINT;
     nUpperBound: DINT;
+    nIndex2: DINT;
+    bMyTurnToExecute: BOOL;
+    abExecuteQueued: ARRAY[1..MotionConstants.MAX_STATE_MOTORS] OF BOOL;
 END_VAR
 ]]></Declaration>
     <Implementation>
@@ -124,13 +127,28 @@ END_IF
         <ST><![CDATA[
 // Do the individual moves
 FOR nIndex := 1 TO nActiveMotorCount DO
+    abExecuteQueued[nIndex] := abExecuteQueued[nIndex] OR bExecute;
+    bMyTurnToExecute := TRUE;
+    IF astPositionState[nIndex].bSequenceMoves THEN
+        FOR nIndex2 := 1 TO nActiveMotorCount DO
+            IF nIndex2 = nIndex THEN
+                continue;
+            ELSIF astPositionState[nIndex2].iSequenceOrder < astPositionState[nIndex].iSequenceOrder AND NOT astMotionStage[nIndex2].bDone THEN
+                bMyTurnToExecute := FALSE;
+                exit;
+            END_IF
+        END_FOR
+    END_IF
     afbPositionStateMove[nIndex](
         stMotionStage:=astMotionStage[nIndex],
         stPositionState:=astPositionState[nIndex],
-        bExecute:=bExecute,
+        bExecute:=abExecuteQueued[nIndex] AND bMyTurnToExecute,
         bReset:=bReset,
         enumMotionRequest:=enumMotionRequest,
     );
+    IF bMyTurnToExecute THEN
+        abExecuteQueued[nIndex] := FALSE;
+    END_IF
 END_FOR
 ]]></ST>
       </Implementation>

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

@@ -4,7 +4,7 @@
     <Declaration><![CDATA[FUNCTION_BLOCK FB_PositionStateReadND
 (*
     Function block to get the combined N-dimensional state of a group of motors.
-    It is a building block not meant for use outside of lcls-twintcat-motion.
+    It is a building block not meant for use outside of lcls-twincat-motion.
 
     Use FB_PositionStateRead1D, FB_PositionStateRead2D, ... etc. instead
 *)
@@ -100,5 +100,21 @@
 ]]></ST>
       </Implementation>
     </Action>
+    <LineIds Name="FB_PositionStateReadND">
+      <LineId Id="3" Count="5" />
+      <LineId Id="2" Count="0" />
+    </LineIds>
+    <LineIds Name="FB_PositionStateReadND.CheckCount">
+      <LineId Id="2" Count="3" />
+      <LineId Id="1" Count="0" />
+    </LineIds>
+    <LineIds Name="FB_PositionStateReadND.CombineOutputs">
+      <LineId Id="2" Count="26" />
+      <LineId Id="1" Count="0" />
+    </LineIds>
+    <LineIds Name="FB_PositionStateReadND.DoStateReads">
+      <LineId Id="2" Count="7" />
+      <LineId Id="1" Count="0" />
+    </LineIds>
   </POU>
 </TcPlcObject>

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

@@ -57,8 +57,47 @@ END_IF
 TestMove(1, 1, 5, 10, FALSE);
 // Somewhere else
 TestMove(2, -10, 0, 5, FALSE);
+
+
+// Move to somewhere in a sequenced manner (motor 2, then motor 3, then motor 1).
+TestMoveSequence(
+    nTestIndex := 3,
+    fMotor1Pos := 20,  iMotor1SequenceOrder := 3,
+    fMotor2Pos := -20,  iMotor2SequenceOrder := 1,
+    fMotor3Pos := 20, iMotor3SequenceOrder := 2,
+    bInterrupt := FALSE);
+(*
+// Move to somewhere else in a different sequence (motors 1 and 2 at the same time, motor 3 after)
+TestMoveSequence(
+    nTestIndex := 4,
+    fMotor1Pos := -10, iMotor1SequenceOrder := 2,
+    fMotor2Pos := -5,  iMotor2SequenceOrder := 1,
+    fMotor3Pos := 1,   iMotor3SequenceOrder := 3,
+    bInterrupt := FALSE);
+// Move to somewhere else in a different sequence (motor 1, then 2, then 3)
+TestMoveSequence(
+    nTestIndex := 5,
+    fMotor1Pos := 1,  iMotor1SequenceOrder := 1,
+    fMotor2Pos := 5,  iMotor2SequenceOrder := 2,
+    fMotor3Pos := 10, iMotor3SequenceOrder := 3,
+    bInterrupt := FALSE);
+// Move to somewhere else in a different sequence (motor 3, then 2, then 1)
+TestMoveSequence(
+    nTestIndex := 6,
+    fMotor1Pos := -10, iMotor1SequenceOrder := 3,
+    fMotor2Pos := 2,   iMotor2SequenceOrder := 2,
+    fMotor3Pos := 4,   iMotor3SequenceOrder := 1,
+    bInterrupt := FALSE);
+// Interrupt on the way to the last place. Make sure all sequence moves are interrupted.
+TestMoveSequence(
+    nTestIndex := 7,
+    fMotor1Pos := 0, iMotor1SequenceOrder := 2,
+    fMotor2Pos := 0, iMotor2SequenceOrder := 3,
+    fMotor3Pos := 0, iMotor3SequenceOrder := 1,
+    bInterrupt := TRUE);*)
+
 // Interrupt on the way to the last place
-TestMove(3, 0, 0, 0, TRUE);
+TestMove(4, 0, 0, 0, TRUE);
 
 IF bOneTestDone THEN
     bOneTestDone := FALSE;
@@ -91,20 +130,23 @@ astGoalPositions[1].fPosition := 0;
 astGoalPositions[1].fDelta := 1;
 astGoalPositions[1].fVelocity := 10;
 astGoalPositions[1].bUseRawCounts := FALSE;
+astGoalPositions[1].bSequenceMoves := FALSE;
 SetGoodState(astGoalPositions[1]);
 
 astGoalPositions[2].sName := 'Goal2';
 astGoalPositions[2].fPosition := 0;
 astGoalPositions[2].fDelta := 1;
 astGoalPositions[2].fVelocity := 10;
 astGoalPositions[2].bUseRawCounts := FALSE;
+astGoalPositions[2].bSequenceMoves := FALSE;
 SetGoodState(astGoalPositions[2]);
 
 astGoalPositions[3].sName := 'Goal3';
 astGoalPositions[3].fPosition := 0;
 astGoalPositions[3].fDelta := 1;
 astGoalPositions[3].fVelocity := 10;
 astGoalPositions[3].bUseRawCounts := FALSE;
+astGoalPositions[3].bSequenceMoves := FALSE;
 SetGoodState(astGoalPositions[3]);
 ]]></ST>
       </Implementation>
@@ -152,6 +194,151 @@ fbMove(
     nActiveMotorCount:=3,
     bExecute:=NOT bInterruptStarted,
 );
+IF fbMove.bDone OR tonTimer.Q OR (bInterruptStarted AND NOT fbMove.bBusy) THEN
+    IF bInterrupt THEN
+        AssertFalse(
+            fbMove.bAtState,
+            Message:='Should have been interrupted, but made it to the goal',
+        );
+    ELSE
+        AssertTrue(
+            fbMove.bAtState,
+            Message:='Did not end at the state',
+        );
+        FOR nIter := 1 TO 3 DO
+            AssertEquals_LREAL(
+                Expected:=astGoalPositions[nIter].fPosition,
+                Actual:=astMotionStage[nIter].stAxisStatus.fActPosition,
+                Delta:=0.01,
+                Message:='Did not reach the goal state',
+            );
+        END_FOR
+
+    END_IF
+    AssertFalse(
+        fbMove.bBusy,
+        Message:='Was busy while done',
+    );
+    AssertFalse(
+        fbMove.bError,
+        Message:='Should not end in error',
+    );
+
+    bOneTestDone := TRUE;
+    TEST_FINISHED();
+END_IF]]></ST>
+      </Implementation>
+    </Method>
+    <Method Name="TestMoveSequence" Id="{05aeaa03-a150-4bf4-b5c8-1ee451d84845}">
+      <Declaration><![CDATA[METHOD TestMoveSequence
+VAR_INPUT
+    nTestIndex: UINT;
+    fMotor1Pos: LREAL;
+    iMotor1SequenceOrder: UINT;
+    fMotor2Pos: LREAL;
+    iMotor2SequenceOrder: UINT;
+    fMotor3Pos: LREAL;
+    iMotor3SequenceOrder: UINT;
+    bInterrupt: BOOL;
+END_VAR
+VAR_INST
+    bLocalInit: BOOL;
+    bInterruptStarted: BOOL;
+    fMotor1StartPos: REAL;
+    bMotor1Moved: BOOL;
+    bMotor1MovedOutOfSequence: BOOL;
+    bMotor1NotDoneMoving: BOOL;
+    bMotor1BeforeMotor2: BOOL;
+    bMotor1BeforeMotor3: BOOL;
+    fMotor2StartPos: REAL;
+    bMotor2Moved: BOOL;
+    bMotor2MovedOutOfSequence: BOOL;
+    bMotor2NotDoneMoving: BOOL;
+    bMotor2BeforeMotor1: BOOL;
+    bMotor2BeforeMotor3: BOOL;
+    fMotor3StartPos: REAL;
+    bMotor3Moved: BOOL;
+    bMotor3MovedOutOfSequence: BOOL;
+    bMotor3NotDoneMoving: BOOL;
+    bMotor3BeforeMotor2: BOOL;
+    bMotor3BeforeMotor1: BOOL;
+END_VAR]]></Declaration>
+      <Implementation>
+        <ST><![CDATA[TEST(CONCAT('TestMoveSequence', UINT_TO_STRING(nTestIndex)));
+IF nTestCounter <> nTestIndex THEN
+    RETURN;
+END_IF
+
+IF NOT bLocalInit THEN
+    // Starting output checks
+    AssertFalse(
+        Condition:=fbMove.bBusy,
+        Message:='Tried to start test with busy motor',
+    );
+    AssertFalse(
+        Condition:=fbMove.bError,
+        Message:='Tried to start test with errored motor',
+    );
+
+    fMotor1StartPos := astMotionStage[1].stAxisStatus.fActPosition;
+    fMotor2StartPos := astMotionStage[2].stAxisStatus.fActPosition;
+    fMotor3StartPos := astMotionStage[3].stAxisStatus.fActPosition;
+
+    bLocalInit := TRUE;
+END_IF
+
+astGoalPositions[1].fPosition := fMotor1Pos;
+astGoalPositions[1].bSequenceMoves := TRUE;
+astGoalPositions[1].iSequenceOrder := iMotor1SequenceOrder;
+astGoalPositions[2].fPosition := fMotor2Pos;
+astGoalPositions[2].bSequenceMoves := TRUE;
+astGoalPositions[2].iSequenceOrder := iMotor2SequenceOrder;
+astGoalPositions[3].fPosition := fMotor3Pos;
+astGoalPositions[3].bSequenceMoves := TRUE;
+astGoalPositions[3].iSequenceOrder := iMotor3SequenceOrder;
+
+//bInterruptStarted S= bInterrupt AND astMotionStage[1].bBusy AND astMotionStage[2].bBusy AND astMotionStage[3].bBusy;
+fbMove(
+    astMotionStage:=astMotionStage,
+    astPositionState:=astGoalPositions,
+    nActiveMotorCount:=3,
+    bExecute:=TRUE //NOT bInterruptStarted,
+);
+
+bMotor1BeforeMotor2 := astGoalPositions[1].iSequenceOrder < astGoalPositions[2].iSequenceOrder;
+bMotor1BeforeMotor3 := astGoalPositions[1].iSequenceOrder < astGoalPositions[3].iSequenceOrder;
+bMotor2BeforeMotor3 := astGoalPositions[2].iSequenceOrder < astGoalPositions[3].iSequenceOrder;
+bMotor2BeforeMotor1 := NOT bMotor1BeforeMotor2;
+bMotor3BeforeMotor2 := NOT bMotor2BeforeMotor3;
+bMotor3BeforeMotor1 := NOT bMotor3BeforeMotor1;
+
+bMotor1NotDoneMoving := NOT astMotionStage[1].bDone;// OR abs(astMotionStage[1].stAxisStatus.fActVelocity) > astMotionStage[1].fVelocity / 10.0;
+bMotor2NotDoneMoving := NOT astMotionStage[2].bDone;// OR abs(astMotionStage[2].stAxisStatus.fActVelocity) > astMotionStage[2].fVelocity / 10.0;
+bMotor3NotDoneMoving := NOT astMotionStage[3].bDone;// OR abs(astMotionStage[3].stAxisStatus.fActVelocity) > astMotionStage[3].fVelocity / 10.0;
+
+bMotor1Moved := ABS(fMotor1StartPos - astMotionStage[1].stAxisStatus.fActPosition) > 0.01;
+bMotor2Moved := ABS(fMotor2StartPos - astMotionStage[2].stAxisStatus.fActPosition) > 0.01;
+bMotor3Moved := ABS(fMotor3StartPos - astMotionStage[3].stAxisStatus.fActPosition) > 0.01;
+
+bMotor1MovedOutOfSequence := bMotor1Moved AND ((bMotor2BeforeMotor1 AND bMotor2NotDoneMoving) OR (bMotor3BeforeMotor1 AND bMotor3NotDoneMoving));
+bMotor2MovedOutOfSequence := bMotor2Moved AND ((bMotor1BeforeMotor2 AND bMotor1NotDoneMoving) OR (bMotor3BeforeMotor2 AND bMotor3NotDoneMoving));
+bMotor3MovedOutOfSequence := bMotor3Moved AND ((bMotor2BeforeMotor3 AND bMotor2NotDoneMoving) OR (bMotor1BeforeMotor3 AND bMotor1NotDoneMoving));
+
+(*
+AssertFalse(
+    bMotor1MovedOutOfSequence,
+    Message:='Motor 1 moved out of the defined sequence',
+);
+AssertFalse(
+    bMotor2MovedOutOfSequence,
+    Message:='Motor 2 moved out of the defined sequence',
+);
+AssertFalse(
+    bMotor3MovedOutOfSequence,
+    Message:='Motor 3 moved out of the defined sequence',
+);
+*)
+
 IF fbMove.bDone OR tonTimer.Q OR (bInterruptStarted AND NOT fbMove.bBusy) THEN
     IF bInterrupt THEN
         AssertFalse(