feat: 575 add drive timer

Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs

@@ -39,6 +39,9 @@ public async Awaitable<bool> ResetDuraOffset()
             var positionResponse = await CommunicationManager.Instance.GetPosition(ManipulatorID);
             if (CommunicationManager.HasError(positionResponse.Error))
                 return false;
+
+            // Save the Dura's position.
+            _duraDepth = positionResponse.Position.w;
 
             // Check if there is enough room for exit margin.
             var continueWithDuraResetCompletionSource = new AwaitableCompletionSource<bool>();
@@ -74,10 +77,9 @@ public async Awaitable<bool> ResetDuraOffset()
             // Force update probe position.
             await UpdateProbePositionFromManipulator();
 
-            // Save the Dura's position.
-            _duraDepth = positionResponse.Position.w;
+            // Save the probe's coordinates at the Dura.
             _duraCoordinate = _probeController.Insertion.APMLDV;
-
+            
             // Log the event.
             OutputLog.Log(
                 new[]

Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs

@@ -36,11 +36,6 @@ public partial class ManipulatorBehaviorController
         /// </summary>
         private const int EXIT_DRIVE_SPEED_MULTIPLIER = 6;
 
-        /// <summary>
-        ///     Speed multiplier of the probe once outside the brain.
-        /// </summary>
-        private const int OUTSIDE_DRIVE_SPEED_MULTIPLIER = 50;
-
         #endregion
 
         #endregion
@@ -53,12 +48,7 @@ public partial class ManipulatorBehaviorController
         /// <remarks>Used to identify which buttons should be made available.</remarks>
         public bool IsMoving { get; private set; }
 
-        #region Caches
-
-        private Vector3 _cachedTargetCoordinate = Vector3.negativeInfinity;
-        private Vector3 _cachedOffsetAdjustedTargetCoordinate = Vector3.negativeInfinity;
-
-        #endregion
+        private float _actualExitMarginToDuraDistance => _duraDepth - _entryCoordinateDepth;
 
         #endregion
 
@@ -110,9 +100,6 @@ float drivePastDistance
                             > NEAR_TARGET_DISTANCE
                         )
                         {
-                            print(
-                                $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth - NEAR_TARGET_DISTANCE}"
-                            );
                             var driveToNearTargetResponse =
                                 await CommunicationManager.Instance.SetDepth(
                                     new SetDepthRequest(
@@ -122,18 +109,13 @@ await CommunicationManager.Instance.SetDepth(
                                     )
                                 );
 
-                            print($"At {driveToNearTargetResponse.Depth}");
-
                             // Shortcut exit if there was an error.
                             if (CommunicationManager.HasError(driveToNearTargetResponse.Error))
                                 return;
                         }
 
                         break;
                     case ProbeAutomationState.DrivingToPastTarget:
-                        print(
-                            $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth + drivePastDistance}"
-                        );
                         // Drive to past target.
                         var driveToPastTargetResponse =
                             await CommunicationManager.Instance.SetDepth(
@@ -144,16 +126,11 @@ await CommunicationManager.Instance.SetDepth(
                                 )
                             );
 
-                        print($"At {driveToPastTargetResponse.Depth}");
-
                         // Shortcut exit if there was an error.
                         if (CommunicationManager.HasError(driveToPastTargetResponse.Error))
                             return;
                         break;
                     case ProbeAutomationState.ReturningToTarget:
-                        print(
-                            $"{ProbeAutomationStateManager.ProbeAutomationState}: Going to {targetDepth}"
-                        );
                         // Drive up to target.
                         var returnToTargetResponse = await CommunicationManager.Instance.SetDepth(
                             new SetDepthRequest(
@@ -163,8 +140,6 @@ await CommunicationManager.Instance.SetDepth(
                             )
                         );
 
-                        print($"At {returnToTargetResponse.Depth}");
-
                         // Shortcut exit if there was an error.
                         if (CommunicationManager.HasError(returnToTargetResponse.Error))
                             return;
@@ -395,13 +370,6 @@ private Vector3 GetOffsetAdjustedTargetCoordinate(ProbeManager targetInsertionPr
             // Extract target insertion.
             var targetInsertion = targetInsertionProbeManager.ProbeController.Insertion;
 
-            // Shortcut exit if already computed and targetInsertion did not change.
-            if (
-                targetInsertion.APMLDV == _cachedTargetCoordinate
-                && !float.IsNegativeInfinity(_cachedOffsetAdjustedTargetCoordinate.x)
-            )
-                return _cachedOffsetAdjustedTargetCoordinate;
-
             var targetWorldT = targetInsertion.PositionWorldT();
             var relativePositionWorldT = _probeController.Insertion.PositionWorldT() - targetWorldT;
             var probeTipTForward = _probeController.ProbeTipT.forward;
@@ -422,11 +390,7 @@ private Vector3 GetOffsetAdjustedTargetCoordinate(ProbeManager targetInsertionPr
                 offsetAdjustedTargetCoordinateAtlasT
             );
 
-            // Cache the computed values.
-            _cachedTargetCoordinate = targetInsertion.APMLDV;
-            _cachedOffsetAdjustedTargetCoordinate = offsetAdjustedTargetCoordinateT;
-
-            return _cachedOffsetAdjustedTargetCoordinate;
+            return offsetAdjustedTargetCoordinateT;
         }
 
         /// <summary>
@@ -465,6 +429,59 @@ private float GetTargetDepth(ProbeManager targetInsertionProbeManager)
             return _duraDepth + GetTargetDistanceToDura(targetInsertionProbeManager);
         }
 
+        /// <summary>
+        ///     Compute the ETA for a probe to reach a target insertion (or exit).
+        /// </summary>
+        /// <param name="targetInsertionProbeManager">Target to calculate ETA to.</param>
+        /// <param name="baseSpeed">Base driving speed in mm/s.</param>
+        /// <param name="drivePastDistance">Distance to drive past target in mm.</param>
+        /// <returns>MM:SS format ETA for reaching a target or exiting, based on the probe's state.</returns>
+        public string GetETA(
+            ProbeManager targetInsertionProbeManager,
+            float baseSpeed,
+            float drivePastDistance
+        )
+        {
+            // Get current distance to target.
+            var distanceToTarget = GetCurrentDistanceToTarget(targetInsertionProbeManager);
+
+            // Compute ETA.
+            var secondsToDestination = ProbeAutomationStateManager.ProbeAutomationState switch
+            {
+                ProbeAutomationState.DrivingToNearTarget
+                    => Mathf.Max(0, distanceToTarget - NEAR_TARGET_DISTANCE) / baseSpeed // To near target.
+                        + (NEAR_TARGET_DISTANCE + 2 * drivePastDistance)
+                            / (baseSpeed * NEAR_TARGET_SPEED_MULTIPLIER), // To past target and back to target.
+                ProbeAutomationState.DrivingToPastTarget
+                    => (distanceToTarget + 2 * drivePastDistance)
+                        / (baseSpeed * NEAR_TARGET_SPEED_MULTIPLIER), // To past target and back to target.
+                ProbeAutomationState.ReturningToTarget
+                    => distanceToTarget / (baseSpeed * NEAR_TARGET_SPEED_MULTIPLIER), // Back to target.
+                ProbeAutomationState.ExitingToDura
+                    => (GetTargetDistanceToDura(targetInsertionProbeManager) - distanceToTarget)
+                        / (baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER) // To Dura.
+                        + DURA_MARGIN_DISTANCE / (baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER) // To exit margin.
+                        + _actualExitMarginToDuraDistance / AUTOMATIC_MOVEMENT_SPEED, // To entry coordinate.
+                ProbeAutomationState.ExitingToMargin
+                    => (
+                        DURA_MARGIN_DISTANCE
+                        - (distanceToTarget - GetTargetDistanceToDura(targetInsertionProbeManager))
+                    ) / (baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER) // To exit margin.
+                        + _actualExitMarginToDuraDistance / AUTOMATIC_MOVEMENT_SPEED, // To entry coordinate.
+                ProbeAutomationState.ExitingToTargetEntryCoordinate
+                    => (
+                        IDEAL_ENTRY_COORDINATE_TO_DURA_DISTANCE
+                        - (distanceToTarget - GetTargetDistanceToDura(targetInsertionProbeManager))
+                    ) / AUTOMATIC_MOVEMENT_SPEED,
+                _ => 0
+            };
+
+            // Return formatted time if above 1 minute.
+            return secondsToDestination < 60
+                ? secondsToDestination.ToString("F1")
+                : TimeSpan.FromSeconds(secondsToDestination).ToString(@"mm\:ss");
+        }
+
         #endregion
     }
 }

Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs

@@ -25,7 +25,7 @@ public partial class ManipulatorBehaviorController
         /// <summary>
         ///     Distance from the entry coordinate to the Dura. This is considered a safe distance to put the probe.
         /// </summary>
-        private const float ENTRY_COORDINATE_DURA_DISTANCE = 3.5f;
+        private const float IDEAL_ENTRY_COORDINATE_TO_DURA_DISTANCE = 3.5f;
 
         #endregion
 
@@ -48,6 +48,12 @@ public partial class ManipulatorBehaviorController
             Vector3.negativeInfinity
         );
 
+        /// <summary>
+        ///     Record the depth at the entry coordinate.
+        /// </summary>
+        /// <remarks>Used during insertion to calculate the actual distance needed to retract back to the entry coordinate.</remarks>
+        private float _entryCoordinateDepth;
+
         #endregion
 
         #region Public Functions
@@ -130,7 +136,7 @@ public async Awaitable<bool> DriveToTargetEntryCoordinate()
             // Shortcut exit if error.
             if (CommunicationManager.HasError(firstMoveResponse.Error))
             {
-                LogDriveToTargetEntryCoordinateProgress("Failed to move to DV position");
+                LogDriveToTargetEntryCoordinateProgress("Failed to move to DV position.");
                 return false;
             }
 
@@ -142,7 +148,7 @@ public async Awaitable<bool> DriveToTargetEntryCoordinate()
             // Shortcut exit if error.
             if (CommunicationManager.HasError(secondMoveResponse.Error))
             {
-                LogDriveToTargetEntryCoordinateProgress("Failed to move to AP position");
+                LogDriveToTargetEntryCoordinateProgress("Failed to move to AP position.");
                 return false;
             }
 
@@ -154,12 +160,26 @@ public async Awaitable<bool> DriveToTargetEntryCoordinate()
             // Shortcut exit if error.
             if (CommunicationManager.HasError(thirdMoveResponse.Error))
             {
-                LogDriveToTargetEntryCoordinateProgress("Failed to move to ML position");
+                LogDriveToTargetEntryCoordinateProgress("Failed to move to ML position.");
                 return false;
             }
 
             // Complete drive.
 
+            // Record depth at entry coordinate.
+            var entryCoordinatePositionResponse = await CommunicationManager.Instance.GetPosition(
+                ManipulatorID
+            );
+            if (CommunicationManager.HasError(entryCoordinatePositionResponse.Error))
+            {
+                LogDriveToTargetEntryCoordinateProgress(
+                    "Failed to get final position at entry coordinate."
+                );
+                return false;
+            }
+
+            _entryCoordinateDepth = entryCoordinatePositionResponse.Position.w;
+
             // Remove trajectory lines.
             RemoveTrajectoryLines();
 
@@ -208,7 +228,7 @@ ProbeManager targetInsertionProbeManager
             var entryCoordinateWorld =
                 targetInsertionProbeManager.GetSurfaceCoordinateWorldT()
                 - targetInsertionProbeManager.ProbeController.GetTipWorldU().tipForwardWorldU
-                    * ENTRY_COORDINATE_DURA_DISTANCE;
+                    * IDEAL_ENTRY_COORDINATE_TO_DURA_DISTANCE;
 
             // Convert world space to transformed space.
             var entryCoordinateAPMLDV = BrainAtlasManager.ActiveAtlasTransform.U2T(

Assets/Scripts/UI/AutomationStack/AutomationStackHandler_Insertion.cs

@@ -1,5 +1,4 @@
 using System;
-using System.Linq;
 using UnityEngine.UIElements;
 
 namespace UI.AutomationStack
@@ -9,31 +8,6 @@ namespace UI.AutomationStack
     /// </summary>
     public partial class AutomationStackHandler
     {
-        #region Properties
-
-        /// <summary>
-        ///     Compute the target insertion probe manager from selected target insertion index.
-        /// </summary>
-        private ProbeManager TargetInsertionProbeManager =>
-            _state.SurfaceCoordinateStringToTargetInsertionOptionProbeManagers[
-                _state.TargetInsertionOptions.ElementAt(_state.SelectedTargetInsertionIndex)
-            ];
-
-        /// <summary>
-        ///     Compute the base speed from selected base speed index.
-        /// </summary>
-        private float BaseSpeed =>
-            _state.SelectedBaseSpeedIndex switch
-            {
-                0 => 0.002f,
-                1 => 0.005f,
-                2 => 0.01f,
-                3 => 0.5f,
-                _ => _state.CustomBaseSpeed / 1000f
-            };
-
-        #endregion
-
         #region Implementations
 
         private partial void OnDriveToTargetInsertionButtonPressed()
@@ -49,9 +23,9 @@ private partial void OnDriveToTargetInsertionButtonPressed()
 
             // Call drive.
             ActiveManipulatorBehaviorController.Drive(
-                TargetInsertionProbeManager,
-                BaseSpeed,
-                _state.DrivePastTargetDistance / 1000f
+                _state.TargetInsertionProbeManager,
+                _state.BaseSpeed,
+                _state.DrivePastTargetDistanceMillimeters
             );
         }
 
@@ -76,7 +50,7 @@ private partial void OnExitButtonPressed()
                 );
 
             // Call exit.
-            ActiveManipulatorBehaviorController.Exit(TargetInsertionProbeManager, BaseSpeed);
+            ActiveManipulatorBehaviorController.Exit(_state.TargetInsertionProbeManager, _state.BaseSpeed);
         }
 
         #endregion

Assets/Scripts/UI/States/AutomationStackState.asset

@@ -14,4 +14,4 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   SelectedBaseSpeedIndex: 1
   CustomBaseSpeed: 20
-  DrivePastTargetDistance: 50
+  DrivePastTargetDistanceMillimeters: 0.05