diff --git a/Assets/Scripts/EphysLink/CommunicationManager.cs b/Assets/Scripts/EphysLink/CommunicationManager.cs index 2cbeda3c..cb4b7723 100644 --- a/Assets/Scripts/EphysLink/CommunicationManager.cs +++ b/Assets/Scripts/EphysLink/CommunicationManager.cs @@ -1,4 +1,5 @@ using System; +using System.IO; using System.Linq; using BestHTTP.SocketIO3; using UnityEngine; @@ -339,6 +340,15 @@ private void GetVersion(Action onSuccessCallback, Action onErrorCallback .Emit("get_version"); } + /// + /// Get Ephys Link version. + /// + /// Version number. + private async Awaitable GetVersion() + { + return await EmitAndGetStringResponse("get_version", null); + } + /// /// Get the platform type. /// @@ -357,6 +367,15 @@ public void GetPlatformType(Action onSuccessCallback, Action onErrorCall .Emit("get_platform_type"); } + /// + /// Get the platform type. + /// + /// Platform type. + public async Awaitable GetPlatformType() + { + return await EmitAndGetStringResponse("get_platform_type", null); + } + /// /// Get manipulators event sender. /// @@ -387,6 +406,18 @@ public void GetManipulators( .Emit("get_manipulators"); } + /// + /// Get connected manipulators and some basic information about them. + /// + /// Manipulators and their information. + public async Awaitable GetManipulators() + { + return await EmitAndGetResponse( + "get_manipulators", + null + ); + } + /// /// Request the current position of a manipulator (mm). /// @@ -419,6 +450,19 @@ public void GetPosition( .Emit("get_position", manipulatorId); } + /// + /// Request the current position of a manipulator (mm). + /// + /// ID of the manipulator to get teh position of. + /// with manipulator's position. + public async Awaitable GetPosition(string manipulatorId) + { + return await EmitAndGetResponse( + "get_position", + manipulatorId + ); + } + /// /// Request the current angles of a manipulator. /// @@ -450,6 +494,16 @@ public void GetAngles( .Emit("get_angles", manipulatorId); } + /// + /// Request the current angles of a manipulator. + /// + /// ID of the manipulator to get the position of + /// with manipulator's angles. + public async Awaitable GetAngles(string manipulatorId) + { + return await EmitAndGetResponse("get_angles", manipulatorId); + } + public void GetShankCount( string manipulatorId, Action onSuccessCallback, @@ -475,6 +529,19 @@ public void GetShankCount( .Emit("get_shank_count", manipulatorId); } + /// + /// Request the number of shanks on a manipulator. + /// + /// ID of the manipulator to get the shank count of. + /// with the number of shanks. + public async Awaitable GetShankCount(string manipulatorId) + { + return await EmitAndGetResponse( + "get_shank_count", + manipulatorId + ); + } + /// /// Request a manipulator be moved to a specific position. /// @@ -507,6 +574,19 @@ public void SetPosition( .Emit("set_position", ToJson(request)); } + /// + /// Request a manipulator be moved to a specific position. + /// + /// Goto position request object + /// with the manipulator's new position. + public async Awaitable SetPosition(SetPositionRequest request) + { + return await EmitAndGetResponse( + "set_position", + request + ); + } + /// /// Request a manipulator drive down to a specific depth. /// @@ -538,6 +618,19 @@ Action onErrorCallback .Emit("set_depth", ToJson(request)); } + /// + /// Request a manipulator drive down to a specific depth. + /// + /// Drive to depth request + /// with the manipulator's new depth. + public async Awaitable SetDepth(SetDepthRequest request) + { + return await EmitAndGetResponse( + "set_depth", + request + ); + } + /// /// Set the inside brain state of a manipulator. /// @@ -570,7 +663,20 @@ public void SetInsideBrain( } /// - /// Request a manipulator stops moving. + /// Set the inside brain state of a manipulator. + /// + /// Set inside brain request. + /// with the manipulator's new inside brain state. + public async Awaitable SetInsideBrain(SetInsideBrainRequest request) + { + return await EmitAndGetResponse( + "set_inside_brain", + request + ); + } + + /// + /// Request a manipulator stops moving. /// /// /// @@ -585,19 +691,25 @@ Action onErrorCallback .Socket.ExpectAcknowledgement(data => { if (DataKnownAndNotEmpty(data)) - { // Non-empty response means error. onErrorCallback?.Invoke(data); - } else - { // Empty response means success. onSuccessCallback?.Invoke(); - } }) .Emit("stop", manipulatorId); } + /// + /// Request a manipulator stops moving. + /// + /// ID of the manipulator to stop + /// Empty string if successful, error message if failed. + public async Awaitable Stop(string manipulatorId) + { + return await EmitAndGetStringResponse("stop", manipulatorId); + } + /// /// Request all movement to stop. /// @@ -609,33 +721,136 @@ public void StopAll(Action onSuccessCallback, Action onErrorCallback) .Socket.ExpectAcknowledgement(data => { if (DataKnownAndNotEmpty(data)) - { // Non-empty response means error. onErrorCallback?.Invoke(data); - } else - { // Empty response means success. onSuccessCallback?.Invoke(); - } }) .Emit("stop_all"); } + /// + /// Request all manipulators to stop. + /// + /// Empty string if successful, error message if failed. + public async Awaitable StopAll() + { + return await EmitAndGetStringResponse("stop_all", null); + } + + #endregion + + #region Utility Functions + + /// + /// Quick error handler to log the error string if it exists. + /// + /// Error response to check. + /// True if there was an error, false otherwise. + public static bool HasError(string error) + { + // Shortcut exit if there was no error. + if (string.IsNullOrEmpty(error)) + return false; + + // Log the error. + Debug.LogError(error); + + // Return true to indicate an error. + return true; + } + #endregion #region Helper functions + /// + /// Generic function to emit and event and get a response from the server. + /// + /// Event to emit to. + /// Parameter to send with the event. + /// Expected (parsed) response type. + /// Type of the request parameter. + /// Response from server. Parsed to if it's not a string. + /// Invalid response from server (empty or unknown). + private async Awaitable EmitAndGetResponse(string eventName, TR requestParameter) + { + // Query server and capture response. + var dataCompletionSource = new AwaitableCompletionSource(); + _connectionManager + .Socket.ExpectAcknowledgement(data => dataCompletionSource.SetResult(data)) + .Emit( + eventName, + typeof(TR) == typeof(string) ? requestParameter : ToJson(requestParameter) + ); + + // Wait for data. + var data = await dataCompletionSource.Awaitable; + + // Return data if it exists. Parse if return type is not string. + if (DataKnownAndNotEmpty(data)) + return ParseJson(data); + + // Throw exception if data is empty. + throw new InvalidDataException($"{eventName} invalid response: {data}"); + } + + /// + /// Emit an event and get a string response from the server. + /// + /// Event to emit to. + /// Parameter to send with the event. + /// Type of the request parameter. + /// Response from server as a string. + private async Awaitable EmitAndGetStringResponse( + string eventName, + TR requestParameter + ) + { + // Query server and capture response. + var dataCompletionSource = new AwaitableCompletionSource(); + _connectionManager + .Socket.ExpectAcknowledgement(data => dataCompletionSource.SetResult(data)) + .Emit( + eventName, + typeof(TR) == typeof(string) ? requestParameter : ToJson(requestParameter) + ); + + // Wait for data. + var data = await dataCompletionSource.Awaitable; + + // Return data. + return data; + } + + /// + /// Check if data is not empty and is not the "unkown event" error. + /// + /// Data to check. + /// True if data is not empty and not the "unkown event" error, false otherwise. private static bool DataKnownAndNotEmpty(string data) { return !string.IsNullOrEmpty(data) && !data.Equals(UNKOWN_EVENT); } + /// + /// Parse a JSON string into a data object. + /// + /// JSON string to parse. + /// Type of the data object. + /// Parsed data object. private static T ParseJson(string json) { return JsonUtility.FromJson(json); } + /// + /// Convert a data object into a JSON string. + /// + /// Data object to convert. + /// Type of the data object. + /// JSON string. private static string ToJson(T data) { return JsonUtility.ToJson(data); diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_BregmaCalibration.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_BregmaCalibration.cs index ab28beca..e5fbe19e 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_BregmaCalibration.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_BregmaCalibration.cs @@ -24,56 +24,67 @@ public partial class ManipulatorBehaviorController /// Alerts user of 4-axis manipulators if depth axis is not at 0 and 3-axis manipulators if depth is too far from /// center. /// - public void ResetZeroCoordinate() + public async Awaitable ResetZeroCoordinate() { - CommunicationManager.Instance.GetPosition( - ManipulatorID, - zeroCoordinate => - { - // Setup alert affirmative callback (continue with reset). - QuestionDialogue.Instance.YesCallback = DoReset; + // Query current position. + var positionalResponse = await CommunicationManager.Instance.GetPosition(ManipulatorID); + + // Shortcut exit if error. + if (CommunicationManager.HasError(positionalResponse.Error)) + return false; + + // Setup callback completion source. + var canDoResetCompletionSource = new AwaitableCompletionSource(); - // Check depth position and alert. - switch (NumAxes) - { - case 3 - when Mathf.Abs(Dimensions.z / 2f - zeroCoordinate.w) - > CENTER_DEVIATION_FACTOR * Dimensions.z: - QuestionDialogue.Instance.NewQuestion( - "The depth axis is too far from the center of its range and may not have enough space to reach the target. Are you sure you want to continue?" - ); - break; - case 4 when Mathf.Approximately(zeroCoordinate.w, 0f): - QuestionDialogue.Instance.NewQuestion( - "The depth axis is not at 0 and may not have enough space to reach the target. Are you sure you want to continue?" - ); - break; - default: - DoReset(); - break; - } + // Setup alert callbacks (continue with reset). + QuestionDialogue.Instance.YesCallback = () => + canDoResetCompletionSource.SetResult(true); + QuestionDialogue.Instance.NoCallback = () => + canDoResetCompletionSource.SetResult(false); - return; + // Check depth position and alert. + switch (NumAxes) + { + case 3 + when Mathf.Abs(Dimensions.z / 2f - positionalResponse.Position.w) + > CENTER_DEVIATION_FACTOR * Dimensions.z: + QuestionDialogue.Instance.NewQuestion( + "The depth axis is too far from the center of its range and may not have enough space to reach the target. Are you sure you want to continue?" + ); + break; + case 4 when Mathf.Approximately(positionalResponse.Position.w, 0f): + QuestionDialogue.Instance.NewQuestion( + "The depth axis is not at 0 and may not have enough space to reach the target. Are you sure you want to continue?" + ); + break; + default: + canDoResetCompletionSource.SetResult(true); + break; + } - void DoReset() - { - ZeroCoordinateOffset = zeroCoordinate; - BrainSurfaceOffset = 0; + // Wait for verification to continue. + var canDoReset = await canDoResetCompletionSource.Awaitable; - // Log event. - OutputLog.Log( - new[] - { - "Copilot", - DateTime.Now.ToString(CultureInfo.InvariantCulture), - "ResetZeroCoordinate", - ManipulatorID, - ZeroCoordinateOffset.ToString() - } - ); - } + // Shortcut exit if reset is canceled. + if (!canDoReset) + return false; + + // Complete reset. + ZeroCoordinateOffset = positionalResponse.Position; + BrainSurfaceOffset = 0; + + // Log event. + OutputLog.Log( + new[] + { + "Copilot", + DateTime.Now.ToString(CultureInfo.InvariantCulture), + "ResetZeroCoordinate", + ManipulatorID, + ZeroCoordinateOffset.ToString() } ); + return true; } } } diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs index 934caa08..e07aafe5 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_DuraCalibration.cs @@ -27,20 +27,19 @@ public partial class ManipulatorBehaviorController /// /// Reset the dura offset of the probe and enable the next step /// - public void ResetDuraOffset() + /// True if the dura offset was reset successfully, false otherwise. + public async Awaitable ResetDuraOffset() { // Reset dura offset. ComputeBrainSurfaceOffset(); // Record the dura depth. - CommunicationManager.Instance.GetPosition( - ManipulatorID, - pos => - { - _duraDepth = pos.w; - _duraCoordinate = _probeController.Insertion.APMLDV; - } - ); + var positionResponse = await CommunicationManager.Instance.GetPosition(ManipulatorID); + if (CommunicationManager.HasError(positionResponse.Error)) + return false; + + _duraDepth = positionResponse.Position.w; + _duraCoordinate = _probeController.Insertion.APMLDV; // Log the event. OutputLog.Log( @@ -53,6 +52,9 @@ public void ResetDuraOffset() BrainSurfaceOffset.ToString(CultureInfo.InvariantCulture) } ); + + // Return success. + return true; } } } diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs index 24cea222..bdfcba35 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_Insertion.cs @@ -72,7 +72,7 @@ public partial class ManipulatorBehaviorController /// Distance to drive past target in mm. /// Probe is not in a drivable state. /// Unhandled probe drive state. - public void Drive( + public async void Drive( ProbeManager targetInsertionProbeManager, float baseSpeed, float drivePastDistance @@ -91,7 +91,7 @@ float drivePastDistance ProbeAutomationStateManager.SetToInsertionDrivingState(); // Log set to driving state. - LogDrive(targetDepth, baseSpeed, drivePastDistance); + LogDriveToTargetInsertion(targetDepth, baseSpeed, drivePastDistance); // Set probe to be moving. IsMoving = true; @@ -105,42 +105,49 @@ float drivePastDistance GetCurrentDistanceToTarget(targetInsertionProbeManager) > NEAR_TARGET_DISTANCE ) - CommunicationManager.Instance.SetDepth( - new SetDepthRequest( - ManipulatorID, - targetDepth - NEAR_TARGET_DISTANCE, - baseSpeed - ), - _ => CompleteDriveStep(), - Debug.LogError - ); - // Skip to next step if already through near target. - else - CompleteDriveStep(); + { + var driveToNearTargetResponse = + await CommunicationManager.Instance.SetDepth( + new SetDepthRequest( + ManipulatorID, + targetDepth - NEAR_TARGET_DISTANCE, + baseSpeed + ) + ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(driveToNearTargetResponse.Error)) + return; + } + break; case ProbeAutomationState.DrivingToPastTarget: // Drive to past target. - CommunicationManager.Instance.SetDepth( + var driveToPastTargetResponse = await CommunicationManager.Instance.SetDepth( new SetDepthRequest( ManipulatorID, targetDepth + drivePastDistance, baseSpeed * NEAR_TARGET_SPEED_MULTIPLIER - ), - _ => CompleteDriveStep(), - Debug.LogError + ) ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(driveToPastTargetResponse.Error)) + return; break; case ProbeAutomationState.ReturningToTarget: // Drive up to target. - CommunicationManager.Instance.SetDepth( + var returnToTargetResponse = await CommunicationManager.Instance.SetDepth( new SetDepthRequest( ManipulatorID, targetDepth, baseSpeed * NEAR_TARGET_SPEED_MULTIPLIER - ), - _ => CompleteDriveStep(), - Debug.LogError + ) ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(returnToTargetResponse.Error)) + return; break; case ProbeAutomationState.IsUncalibrated: case ProbeAutomationState.IsCalibrated: @@ -167,54 +174,47 @@ float drivePastDistance ); } - return; + // Increment cycle state. + ProbeAutomationStateManager.IncrementInsertionCycleState(); - // Increment the state in the insertion cycle and call drive if not at target yet. - void CompleteDriveStep() - { - // Increment cycle state. - ProbeAutomationStateManager.IncrementInsertionCycleState(); - - // Log the event. - LogDrive(targetDepth, baseSpeed, drivePastDistance); - - // Call drive if not at target yet. - if ( - ProbeAutomationStateManager.ProbeAutomationState - != ProbeAutomationState.AtTarget - ) - Drive(targetInsertionProbeManager, baseSpeed, drivePastDistance); - // Set probe to be done moving. - else - IsMoving = false; - } + // Log the event. + LogDriveToTargetInsertion(targetDepth, baseSpeed, drivePastDistance); + + // Call drive if not at target yet. + if (ProbeAutomationStateManager.ProbeAutomationState != ProbeAutomationState.AtTarget) + Drive(targetInsertionProbeManager, baseSpeed, drivePastDistance); + // Set probe to be done moving. + else + IsMoving = false; } /// /// Stop the probe's movement. /// - public void Stop() + public async void Stop() { - CommunicationManager.Instance.Stop( - ManipulatorID, - () => + var stopResponse = await CommunicationManager.Instance.Stop(ManipulatorID); + + // Log and exit if there was an error. + if (!string.IsNullOrEmpty(stopResponse)) + { + Debug.LogError(stopResponse); + return; + } + + // Set probe to be not moving. + IsMoving = false; + + // Log stop event. + OutputLog.Log( + new[] { - // Set probe to be not moving. - IsMoving = false; - - // Log stop event. - OutputLog.Log( - new[] - { - "Automation", - DateTime.Now.ToString(CultureInfo.InvariantCulture), - "Drive", - ManipulatorID, - "Stop" - } - ); - }, - Debug.LogError + "Automation", + DateTime.Now.ToString(CultureInfo.InvariantCulture), + "Drive", + ManipulatorID, + "Stop" + } ); } @@ -225,7 +225,7 @@ public void Stop() /// Base driving speed in mm/s. /// Probe is not in an exitable state. /// Unhandled probe exit state. - public void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed) + public async void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed) { // Throw exception if state is not valid. if (!ProbeAutomationStateManager.IsExitable()) @@ -240,7 +240,7 @@ public void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed) ProbeAutomationStateManager.SetToExitingDrivingState(); // Log set to exiting state. - LogDrive(targetDepth, baseSpeed); + LogDriveToTargetInsertion(targetDepth, baseSpeed); // Set probe to be moving. IsMoving = true; @@ -254,58 +254,67 @@ public void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed) GetCurrentDistanceToTarget(targetInsertionProbeManager) < NEAR_TARGET_DISTANCE ) - CommunicationManager.Instance.SetDepth( + { + var exitToNearTargetResponse = await CommunicationManager.Instance.SetDepth( new SetDepthRequest( ManipulatorID, targetDepth - NEAR_TARGET_DISTANCE, baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER * NEAR_TARGET_SPEED_MULTIPLIER - ), - _ => CompleteExitStep(), - Debug.LogError + ) ); - // Skip to next step if already above near target. - else - CompleteExitStep(); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(exitToNearTargetResponse.Error)) + return; + } + break; case ProbeAutomationState.ExitingToDura: // Exit back up to the Dura. - CommunicationManager.Instance.SetDepth( + var exitToDuaraResponse = await CommunicationManager.Instance.SetDepth( new SetDepthRequest( ManipulatorID, _duraDepth, baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER - ), - _ => CompleteExitStep(), - Debug.LogError + ) ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(exitToDuaraResponse.Error)) + return; break; case ProbeAutomationState.ExitingToMargin: // Exit to the safe margin above the Dura. - CommunicationManager.Instance.SetDepth( + var exitToMarginResponse = await CommunicationManager.Instance.SetDepth( new SetDepthRequest( ManipulatorID, _duraDepth - DURA_MARGIN_DISTANCE, baseSpeed * EXIT_DRIVE_SPEED_MULTIPLIER - ), - _ => CompleteExitStep(), - Debug.LogError + ) ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(exitToMarginResponse.Error)) + return; break; case ProbeAutomationState.ExitingToTargetEntryCoordinate: // Drive to the target entry coordinate (same place before calibrating to the Dura). - CommunicationManager.Instance.SetPosition( - new SetPositionRequest( - ManipulatorID, - ConvertInsertionAPMLDVToManipulatorPosition( - _trajectoryCoordinates.third - ), - AUTOMATIC_MOVEMENT_SPEED - ), - _ => CompleteExitStep(), - Debug.LogError - ); + var exitToTargetEntryCoordinateResponse = + await CommunicationManager.Instance.SetPosition( + new SetPositionRequest( + ManipulatorID, + ConvertInsertionAPMLDVToManipulatorPosition( + _trajectoryCoordinates.third + ), + AUTOMATIC_MOVEMENT_SPEED + ) + ); + + // Shortcut exit if there was an error. + if (CommunicationManager.HasError(exitToTargetEntryCoordinateResponse.Error)) + return; break; case ProbeAutomationState.IsUncalibrated: case ProbeAutomationState.IsCalibrated: @@ -331,31 +340,25 @@ public void Exit(ProbeManager targetInsertionProbeManager, float baseSpeed) ); } - return; - - // Increment the state in the insertion cycle and call exit if not at entry coordinate yet. - void CompleteExitStep() - { - // Increment cycle state. - ProbeAutomationStateManager.IncrementInsertionCycleState(); + // Increment cycle state. + ProbeAutomationStateManager.IncrementInsertionCycleState(); - // Log the event. - LogDrive(targetDepth, baseSpeed); + // Log the event. + LogDriveToTargetInsertion(targetDepth, baseSpeed); - // Call exit if not back at entry coordinate yet. - if ( - ProbeAutomationStateManager.ProbeAutomationState - != ProbeAutomationState.AtEntryCoordinate - ) - { - Exit(targetInsertionProbeManager, baseSpeed); - } - // Set probe to be done moving and remove Dura offset. - else - { - IsMoving = false; - BrainSurfaceOffset = 0; - } + // Call exit if not back at entry coordinate yet. + if ( + ProbeAutomationStateManager.ProbeAutomationState + != ProbeAutomationState.AtEntryCoordinate + ) + { + Exit(targetInsertionProbeManager, baseSpeed); + } + // Set probe to be done moving and remove Dura offset. + else + { + IsMoving = false; + BrainSurfaceOffset = 0; } } @@ -369,14 +372,18 @@ void CompleteExitStep() /// Target depth of drive. /// Base speed of drive. /// Distance (mm) driven past the target. Only supplied in insertion drives. - private void LogDrive(float targetDepth, float baseSpeed, float drivePastDistance = 0) + private void LogDriveToTargetInsertion( + float targetDepth, + float baseSpeed, + float drivePastDistance = 0 + ) { OutputLog.Log( new[] { "Automation", DateTime.Now.ToString(CultureInfo.InvariantCulture), - "Drive", + "DriveToTargetInsertion", ManipulatorID, ProbeAutomationStateManager.ProbeAutomationState.ToString(), targetDepth.ToString(CultureInfo.InvariantCulture), diff --git a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs index 84c047f3..1c9fefb6 100644 --- a/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs +++ b/Assets/Scripts/Pinpoint/Probes/ManipulatorBehaviorController/ManipulatorBehaviorController_Automation_TargetInsertion.cs @@ -88,10 +88,10 @@ public Vector3 ComputeEntryCoordinateTrajectory(ProbeManager targetInsertionProb /// /// Move the probe along the planned trajectory to the target entry coordinate.
/// - /// Callback action after movement is completed. /// No trajectory planned for probe /// Will log that movement has started and completed. - public void DriveToTargetEntryCoordinate(Action onDriveEnd) + /// True if movement was successful, false otherwise. + public async Awaitable DriveToTargetEntryCoordinate() { // Throw exception if invariant is violated. if (float.IsNegativeInfinity(_trajectoryCoordinates.first.x)) @@ -100,9 +100,15 @@ public void DriveToTargetEntryCoordinate(Action onDriveEnd) ); // Convert insertions to manipulator positions. - var dvPosition = ConvertInsertionAPMLDVToManipulatorPosition(_trajectoryCoordinates.first); - var apPosition = ConvertInsertionAPMLDVToManipulatorPosition(_trajectoryCoordinates.second); - var mlPosition = ConvertInsertionAPMLDVToManipulatorPosition(_trajectoryCoordinates.third); + var dvPosition = ConvertInsertionAPMLDVToManipulatorPosition( + _trajectoryCoordinates.first + ); + var apPosition = ConvertInsertionAPMLDVToManipulatorPosition( + _trajectoryCoordinates.second + ); + var mlPosition = ConvertInsertionAPMLDVToManipulatorPosition( + _trajectoryCoordinates.third + ); // Log that movement is starting. OutputLog.Log( @@ -116,92 +122,74 @@ public void DriveToTargetEntryCoordinate(Action onDriveEnd) } ); - // Move. - CommunicationManager.Instance.SetPosition( - new SetPositionRequest(ManipulatorID, dvPosition, AUTOMATIC_MOVEMENT_SPEED), - _ => - { - CommunicationManager.Instance.SetPosition( - new SetPositionRequest(ManipulatorID, apPosition, AUTOMATIC_MOVEMENT_SPEED), - _ => - { - CommunicationManager.Instance.SetPosition( - new SetPositionRequest( - ManipulatorID, - mlPosition, - AUTOMATIC_MOVEMENT_SPEED - ), - _ => - { - // Remove trajectory lines. - RemoveTrajectoryLines(); - - // Conclude drive. - ConcludeDrive(); - }, - error => - { - Debug.LogError(error); - ConcludeDrive(); - } - ); - }, - error => - { - Debug.LogError(error); - ConcludeDrive(); - } - ); - }, - error => - { - Debug.LogError(error); - ConcludeDrive(); - } + // First move. + var firstMoveResponse = await CommunicationManager.Instance.SetPosition( + new SetPositionRequest(ManipulatorID, dvPosition, AUTOMATIC_MOVEMENT_SPEED) ); - return; + // Shortcut exit if error. + if (CommunicationManager.HasError(firstMoveResponse.Error)) + { + LogDriveToTargetEntryCoordinateProgress("Failed to move to DV position"); + return false; + } - void ConcludeDrive() + // Second move. + var secondMoveResponse = await CommunicationManager.Instance.SetPosition( + new SetPositionRequest(ManipulatorID, apPosition, AUTOMATIC_MOVEMENT_SPEED) + ); + + // Shortcut exit if error. + if (CommunicationManager.HasError(secondMoveResponse.Error)) { - // Log drive finished. - OutputLog.Log( - new[] - { - "Automation", - DateTime.Now.ToString(CultureInfo.InvariantCulture), - "DriveToTargetEntryCoordinate", - ManipulatorID, - "Finish" - } - ); + LogDriveToTargetEntryCoordinateProgress("Failed to move to AP position"); + return false; + } + + // Third move. + var thirdMoveResponse = await CommunicationManager.Instance.SetPosition( + new SetPositionRequest(ManipulatorID, mlPosition, AUTOMATIC_MOVEMENT_SPEED) + ); - // Callback drive end. - onDriveEnd.Invoke(); + // Shortcut exit if error. + if (CommunicationManager.HasError(thirdMoveResponse.Error)) + { + LogDriveToTargetEntryCoordinateProgress("Failed to move to ML position"); + return false; } + + // Complete drive. + + // Remove trajectory lines. + RemoveTrajectoryLines(); + + // Log drive finished. + LogDriveToTargetEntryCoordinateProgress("Finish"); + + // Return success. + return true; } /// /// Stop the probe from moving to the target entry coordinate. /// /// Will log that movement has stopped. - /// Callback action after movement is stopped. - public void StopDriveToTargetEntryCoordinate(Action onStopped) + /// True if movement was stopped successfully, false otherwise. + public async Awaitable StopDriveToTargetEntryCoordinate() { // Log that movement is stopping. - OutputLog.Log( - new[] - { - "Copilot", - DateTime.Now.ToString(CultureInfo.InvariantCulture), - "MoveToTargetInsertion", - ManipulatorID, - "Stop" - } - ); + LogDriveToTargetEntryCoordinateProgress("Stopped"); // Stop movement. - CommunicationManager.Instance.Stop(ManipulatorID, onStopped.Invoke, Debug.LogError); + var stopResponse = await CommunicationManager.Instance.Stop(ManipulatorID); + + // Shortcut exit if no errors. + if (string.IsNullOrEmpty(stopResponse)) + return true; + + // Log errors. + Debug.LogError(stopResponse); + return false; } #endregion @@ -355,6 +343,24 @@ private void RemoveTrajectoryLines() _trajectoryLineLineRenderers = (null, null, null); } + /// + /// Log the progress of the drive to the target entry coordinate. + /// + /// Message to log + private void LogDriveToTargetEntryCoordinateProgress(string progressMessage) + { + OutputLog.Log( + new[] + { + "Automation", + DateTime.Now.ToString(CultureInfo.InvariantCulture), + "DriveToTargetEntryCoordinate", + ManipulatorID, + progressMessage + } + ); + } + #endregion } } diff --git a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_BregmaCalibration.cs b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_BregmaCalibration.cs index 8bcb5bcc..e4306981 100644 --- a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_BregmaCalibration.cs +++ b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_BregmaCalibration.cs @@ -7,7 +7,7 @@ namespace UI.AutomationStack /// public partial class AutomationStackHandler { - private partial void ResetBregmaCalibration() + private async partial void ResetBregmaCalibration() { // Throw exception if invariant is violated. if (!_state.IsEnabled) @@ -17,10 +17,9 @@ private partial void ResetBregmaCalibration() ); // Reset the Bregma calibration of the active probe manager. - ActiveManipulatorBehaviorController.ResetZeroCoordinate(); - - // Set probe's automation state to be calibrated. - ActiveProbeStateManager.SetCalibrated(); + if (await ActiveManipulatorBehaviorController.ResetZeroCoordinate()) + // Set probe's automation state to be calibrated if it did happen. + ActiveProbeStateManager.SetCalibrated(); } } } diff --git a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_DuraCalibration.cs b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_DuraCalibration.cs index 14075333..fa8ef9b8 100644 --- a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_DuraCalibration.cs +++ b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_DuraCalibration.cs @@ -7,19 +7,18 @@ namespace UI.AutomationStack /// public partial class AutomationStackHandler { - private partial void OnResetDuraCalibrationPressed() + private async partial void OnResetDuraCalibrationPressed() { if (!_state.IsEnabled) throw new InvalidOperationException( "Cannot reset Dura calibration if automation is not enabled on probe " + ProbeManager.ActiveProbeManager.name ); - + // Reset Dura calibration on the active probe manager. - ActiveManipulatorBehaviorController.ResetDuraOffset(); - - // Set probe's automation state to be at Dura. - ActiveProbeStateManager.SetAtDuraInsert(); + if (await ActiveManipulatorBehaviorController.ResetDuraOffset()) + // Set probe's automation state to be at Dura. + ActiveProbeStateManager.SetAtDuraInsert(); } } } diff --git a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_TargetInsertion.cs b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_TargetInsertion.cs index fa7f021d..d2d0523f 100644 --- a/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_TargetInsertion.cs +++ b/Assets/Scripts/UI/AutomationStack/AutomationStackHandler_TargetInsertion.cs @@ -172,7 +172,7 @@ void CheckFinalInsertionIsOutOfBounds() } } - private partial void OnDriveToTargetEntryCoordinatePressed() + private async partial void OnDriveToTargetEntryCoordinatePressed() { // Throw exception if invariant is violated. if (!_state.IsEnabled || !ActiveProbeStateManager.IsCalibrated()) @@ -181,12 +181,15 @@ private partial void OnDriveToTargetEntryCoordinatePressed() ); // If the probe is moving, call stop. - if (ActiveProbeStateManager.ProbeAutomationState == ProbeAutomationState.DrivingToTargetEntryCoordinate) + if ( + ActiveProbeStateManager.ProbeAutomationState + == ProbeAutomationState.DrivingToTargetEntryCoordinate + ) { - ActiveManipulatorBehaviorController.StopDriveToTargetEntryCoordinate( - // On stop, set the probe back to calibrated state. - () => ActiveProbeStateManager.SetCalibrated() - ); + // Call stop and wait. + if (await ActiveManipulatorBehaviorController.StopDriveToTargetEntryCoordinate()) + // Reset probe to be calibrated on stop. + ActiveProbeStateManager.SetCalibrated(); } else { @@ -194,10 +197,9 @@ private partial void OnDriveToTargetEntryCoordinatePressed() ActiveProbeStateManager.SetDrivingToTargetEntryCoordinate(); // Send drive command. - ActiveManipulatorBehaviorController.DriveToTargetEntryCoordinate( - // On completion, set probe to at entry coordinate. - () => ActiveProbeStateManager.SetAtEntryCoordinate() - ); + if (await ActiveManipulatorBehaviorController.DriveToTargetEntryCoordinate()) + // Mark as at entry coordinate when completed. + ActiveProbeStateManager.SetAtEntryCoordinate(); } }