diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs index 0ded0617..e59d481d 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs @@ -39,6 +39,9 @@ public async Awaitable 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(); @@ -74,10 +77,9 @@ public async Awaitable 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[] diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs index 21ab7cb9..8e134789 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs @@ -36,11 +36,6 @@ public partial class ManipulatorBehaviorController /// private const int EXIT_DRIVE_SPEED_MULTIPLIER = 6; - /// - /// Speed multiplier of the probe once outside the brain. - /// - private const int OUTSIDE_DRIVE_SPEED_MULTIPLIER = 50; - #endregion #endregion @@ -53,12 +48,7 @@ public partial class ManipulatorBehaviorController /// Used to identify which buttons should be made available. 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,8 +109,6 @@ await CommunicationManager.Instance.SetDepth( ) ); - print($"At {driveToNearTargetResponse.Depth}"); - // Shortcut exit if there was an error. if (CommunicationManager.HasError(driveToNearTargetResponse.Error)) return; @@ -131,9 +116,6 @@ await CommunicationManager.Instance.SetDepth( 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; } /// @@ -465,6 +429,59 @@ private float GetTargetDepth(ProbeManager targetInsertionProbeManager) return _duraDepth + GetTargetDistanceToDura(targetInsertionProbeManager); } + /// + /// Compute the ETA for a probe to reach a target insertion (or exit). + /// + /// Target to calculate ETA to. + /// Base driving speed in mm/s. + /// Distance to drive past target in mm. + /// MM:SS format ETA for reaching a target or exiting, based on the probe's state. + 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 } } diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs index 1c9fefb6..0279209d 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs @@ -25,7 +25,7 @@ public partial class ManipulatorBehaviorController /// /// Distance from the entry coordinate to the Dura. This is considered a safe distance to put the probe. /// - 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 ); + /// + /// Record the depth at the entry coordinate. + /// + /// Used during insertion to calculate the actual distance needed to retract back to the entry coordinate. + private float _entryCoordinateDepth; + #endregion #region Public Functions @@ -130,7 +136,7 @@ public async Awaitable 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 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 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( diff --git a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_Insertion.cs b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_Insertion.cs index 0d10162f..d42b8cf3 100644 --- a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_Insertion.cs +++ b/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 /// public partial class AutomationStackHandler { - #region Properties - - /// - /// Compute the target insertion probe manager from selected target insertion index. - /// - private ProbeManager TargetInsertionProbeManager => - _state.SurfaceCoordinateStringToTargetInsertionOptionProbeManagers[ - _state.TargetInsertionOptions.ElementAt(_state.SelectedTargetInsertionIndex) - ]; - - /// - /// Compute the base speed from selected base speed index. - /// - 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 diff --git a/Assets/Scripts/UI/States/AutomationStackState.asset b/Assets/Scripts/UI/States/AutomationStackState.asset index 21170fe1..8f19a0dd 100644 --- a/Assets/Scripts/UI/States/AutomationStackState.asset +++ b/Assets/Scripts/UI/States/AutomationStackState.asset @@ -14,4 +14,4 @@ MonoBehaviour: m_EditorClassIdentifier: SelectedBaseSpeedIndex: 1 CustomBaseSpeed: 20 - DrivePastTargetDistance: 50 + DrivePastTargetDistanceMillimeters: 0.05 diff --git a/Assets/Scripts/UI/States/AutomationStackState.cs b/Assets/Scripts/UI/States/AutomationStackState.cs index fc82ea68..5df9212b 100644 --- a/Assets/Scripts/UI/States/AutomationStackState.cs +++ b/Assets/Scripts/UI/States/AutomationStackState.cs @@ -237,6 +237,16 @@ out var selectedTargetInsertionProbeManager #region Option List helpers + /// + /// Compute the target insertion probe manager from selected target insertion index. + /// + public ProbeManager TargetInsertionProbeManager => + SelectedTargetInsertionIndex > 0 + ? SurfaceCoordinateStringToTargetInsertionOptionProbeManagers[ + TargetInsertionOptions.ElementAt(SelectedTargetInsertionIndex) + ] + : null; + /// /// Expose mapping from target insertion option probe manager to surface coordinate string. /// @@ -410,17 +420,46 @@ public float DuraCalibrationOffset #region Insertion + /// + /// Is the base speed radio group enabled for editing. + /// + /// True when the selected probe is Ephys Link controlled and not moving, false otherwise. + [CreateProperty] + public bool IsBaseSpeedRadioGroupEnabled => + IsEnabled && !ActiveManipulatorBehaviorController.IsMoving; + /// /// Selection index in radio button group for base insertion speed. /// public int SelectedBaseSpeedIndex; + /// + /// Is the custom base speed field enabled. + /// + /// True when the selected probe is Ephys Link controlled and not moving, false otherwise. + [CreateProperty] + public bool IsCustomBaseSpeedEnabled => + IsEnabled && !ActiveManipulatorBehaviorController.IsMoving; + /// /// Custom base insertion speed (µm/s). /// /// Should only be used when is on "Custom" index. public int CustomBaseSpeed; + /// + /// Compute the base speed from selected base speed index. + /// + public float BaseSpeed => + SelectedBaseSpeedIndex switch + { + 0 => 0.002f, + 1 => 0.005f, + 2 => 0.01f, + 3 => 0.5f, + _ => CustomBaseSpeed / 1000f + }; + /// /// Visibility of the custom insertion base speed field. /// @@ -429,10 +468,29 @@ public float DuraCalibrationOffset public DisplayStyle CustomInsertionBaseSpeedDisplayStyle => SelectedBaseSpeedIndex == 4 ? DisplayStyle.Flex : DisplayStyle.None; + /// + /// Is the drive past target distance field enabled. + /// + /// True when the selected probe is Ephys Link controlled and not moving, false otherwise. + [CreateProperty] + public bool IsDrivePastTargetDistanceEnabled => + IsEnabled && !ActiveManipulatorBehaviorController.IsMoving; + + /// + /// Distance to drive past the target insertion depth (mm). + /// + public float DrivePastTargetDistanceMillimeters; + /// /// Distance to drive past the target insertion depth (µm). /// - public int DrivePastTargetDistance; + /// Used in UI since these are small numbers. + [CreateProperty] + public int DrivePastTargetDistanceMicrometers + { + get => Mathf.RoundToInt(DrivePastTargetDistanceMillimeters * 1000); + set => DrivePastTargetDistanceMillimeters = value / 1000f; + } /// /// Is the drive to target insertion button enabled. @@ -503,6 +561,28 @@ public float DuraCalibrationOffset ? DisplayStyle.Flex : DisplayStyle.None; + [CreateProperty] + public string ETA => + IsEnabled && SelectedTargetInsertionIndex > 0 + ? $"ETA: {ActiveManipulatorBehaviorController.GetETA(TargetInsertionProbeManager, BaseSpeed, DrivePastTargetDistanceMillimeters)}" + : "ETA: N/A"; + + /// + /// Visibility of the ETA label. + /// + /// + /// Shown only when selected/active probe is Ephys Link controlled, the probe is moving, and the probe is in the + /// brain. + /// + [CreateProperty] + public DisplayStyle ETADisplayStyle => + IsEnabled + && ActiveManipulatorBehaviorController.IsMoving + && ActiveProbeAutomationStateManager.ProbeAutomationState + >= ProbeAutomationState.AtDuraInsert + ? DisplayStyle.Flex + : DisplayStyle.None; + #endregion } } diff --git a/Assets/UI/Components/AutomationStack.uxml b/Assets/UI/Components/AutomationStack.uxml index 5331b348..6bd9838a 100644 --- a/Assets/UI/Components/AutomationStack.uxml +++ b/Assets/UI/Components/AutomationStack.uxml @@ -68,19 +68,27 @@ + + - + + + + + + +