From b05bc1e9b6e1a3e2ab1e8b41b2128c7295ceb8df Mon Sep 17 00:00:00 2001 From: Kenneth Yang <82800265+kjy5@users.noreply.github.com> Date: Wed, 7 Feb 2024 15:06:52 -0800 Subject: [PATCH] 321 detect stopped movement from controller and stop all movements (#322) * Detect in Sensapex ump4 * Report if New Scale and uMp-3 did not make it to target --- src/ephys_link/platform_handler.py | 4 ++-- src/ephys_link/platform_manipulator.py | 1 + .../platforms/new_scale_manipulator.py | 17 +++++++++++++++++ .../platforms/sensapex_manipulator.py | 16 +++++++++++++--- src/ephys_link/platforms/ump3_manipulator.py | 10 ++++++++++ 5 files changed, 43 insertions(+), 5 deletions(-) diff --git a/src/ephys_link/platform_handler.py b/src/ephys_link/platform_handler.py index 1f7b7b0..d104c7a 100644 --- a/src/ephys_link/platform_handler.py +++ b/src/ephys_link/platform_handler.py @@ -421,11 +421,11 @@ def _get_shank_count(self, manipulator_id: str) -> com.ShankCountOutputData: raise NotImplementedError @abstractmethod - async def _goto_pos(self, manipulator_id: str, position: list[float], speed: int) -> com.PositionalOutputData: + async def _goto_pos(self, manipulator_id: str, position: list[float], speed: float) -> com.PositionalOutputData: raise NotImplementedError @abstractmethod - async def _drive_to_depth(self, manipulator_id: str, depth: float, speed: int) -> com.DriveToDepthOutputData: + async def _drive_to_depth(self, manipulator_id: str, depth: float, speed: float) -> com.DriveToDepthOutputData: raise NotImplementedError @abstractmethod diff --git a/src/ephys_link/platform_manipulator.py b/src/ephys_link/platform_manipulator.py index f7c0dc9..066bc01 100644 --- a/src/ephys_link/platform_manipulator.py +++ b/src/ephys_link/platform_manipulator.py @@ -18,6 +18,7 @@ def __init__(self): """Initialize manipulator.""" self._id = None + self._movement_tolerance = 0.001 self._calibrated = False self._inside_brain = False self._can_write = False diff --git a/src/ephys_link/platforms/new_scale_manipulator.py b/src/ephys_link/platforms/new_scale_manipulator.py index fad857a..5274b31 100644 --- a/src/ephys_link/platforms/new_scale_manipulator.py +++ b/src/ephys_link/platforms/new_scale_manipulator.py @@ -149,8 +149,18 @@ async def goto_pos(self, position: list[float], speed: float) -> com.PositionalO # Return success unless write was disabled during movement (meaning a stop occurred) if not self._can_write: + com.dprint(f"[ERROR]\t\t Manipulator {self._id} movement canceled") return com.PositionalOutputData([], "Manipulator movement canceled") + # Return error if movement did not reach target. + if not all( + abs(manipulator_final_position[i] - position[i]) < self._movement_tolerance + for i in range(len(position)) + ): + com.dprint(f"[ERROR]\t\t Manipulator {self._id} did not reach target position") + return com.PositionalOutputData([], "Manipulator did not reach target position") + + # Made it to the target. com.dprint(f"[SUCCESS]\t Moved manipulator {self._id} to position" f" {manipulator_final_position}\n") return com.PositionalOutputData(manipulator_final_position, "") except Exception as e: @@ -208,8 +218,15 @@ async def drive_to_depth(self, depth: float, speed: float) -> com.DriveToDepthOu # Return success unless write was disabled during movement (meaning a stop occurred) if not self._can_write: + com.dprint(f"[ERROR]\t\t Manipulator {self._id} movement canceled") return com.DriveToDepthOutputData(0, "Manipulator movement canceled") + # Return error if movement did not reach target. + if not abs(manipulator_final_position[3] - depth) < self._movement_tolerance: + com.dprint(f"[ERROR]\t\t Manipulator {self._id} did not reach target depth") + return com.DriveToDepthOutputData(0, "Manipulator did not reach target depth") + + # Made it to the target. com.dprint(f"[SUCCESS]\t Moved manipulator {self._id} to position" f" {manipulator_final_position}\n") return com.DriveToDepthOutputData(manipulator_final_position[3], "") except Exception as e: diff --git a/src/ephys_link/platforms/sensapex_manipulator.py b/src/ephys_link/platforms/sensapex_manipulator.py index 2cab038..218631d 100644 --- a/src/ephys_link/platforms/sensapex_manipulator.py +++ b/src/ephys_link/platforms/sensapex_manipulator.py @@ -98,14 +98,24 @@ async def goto_pos(self, position: list[float], speed: float) -> com.PositionalO # Get position manipulator_final_position = self.get_pos()["position"] - # Mark movement as finished + # Mark movement as finished. self._is_moving = False - # Return success unless write was disabled during movement (meaning a stop occurred) + # Return success unless write was disabled during movement (meaning a stop occurred). if not self._can_write: + com.dprint(f"[ERROR]\t\t Manipulator {self._id} movement canceled") return com.PositionalOutputData([], "Manipulator movement canceled") - com.dprint(f"[SUCCESS]\t Moved manipulator {self._id} to position" f" {manipulator_final_position}\n") + # Return error if movement did not reach target. + if not all( + abs(manipulator_final_position[i] - position[i]) < self._movement_tolerance + for i in range(len(position)) + ): + com.dprint(f"[ERROR]\t\t Manipulator {self._id} did not reach target position") + return com.PositionalOutputData([], "Manipulator did not reach target position") + + # Made it to the target. + com.dprint(f"[SUCCESS]\t Moved manipulator {self._id} to position {manipulator_final_position}\n") return com.PositionalOutputData(manipulator_final_position, "") except Exception as e: print(f"[ERROR]\t\t Moving manipulator {self._id} to position" f" {position}") diff --git a/src/ephys_link/platforms/ump3_manipulator.py b/src/ephys_link/platforms/ump3_manipulator.py index ccc5e6f..d0c8dfb 100644 --- a/src/ephys_link/platforms/ump3_manipulator.py +++ b/src/ephys_link/platforms/ump3_manipulator.py @@ -100,8 +100,18 @@ async def goto_pos(self, position: list[float], speed: float) -> com.PositionalO # Return success unless write was disabled during movement (meaning a stop occurred) if not self._can_write: + com.dprint(f"[ERROR]\t\t Manipulator {self._id} movement canceled") return com.PositionalOutputData([], "Manipulator movement canceled") + # Return error if movement did not reach target. + if not all( + abs(manipulator_final_position[i] - position[i]) < self._movement_tolerance + for i in range(len(position)) + ): + com.dprint(f"[ERROR]\t\t Manipulator {self._id} did not reach target position") + return com.PositionalOutputData([], "Manipulator did not reach target position") + + # Made it to the target. com.dprint(f"[SUCCESS]\t Moved manipulator {self._id} to position" f" {manipulator_final_position}\n") return com.PositionalOutputData(manipulator_final_position, "") except Exception as e: