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

@@ -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

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

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:

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}")

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: