Refactor wait_for_arrival() to include timeout (#233)

* adds 10ms pause within wait_for_arrive(); set's old firmware coordinate after limit hit

* adds timeouts to Driver.wait_for_arrival() incase no movement occurs

* pylama

* adds tests

* pylama

* attach robot to instrumentMotor and Mosfet incase user switches between firmware version

Author: andySigler

api/opentrons/drivers/smoothie_drivers/v1_2_0/driver.py

@@ -289,8 +289,12 @@ def detect_limit_hit(self, msg):
         """
         if '!!' in msg or 'limit' in msg:
             log.debug('home switch hit')
+            time.sleep(0.5)
             self.flush_port()
             self.calm_down()
+            self.set_position(
+                **self.get_position().get('current', {l: 0 for l in 'xyzab'})
+            )
             raise RuntimeWarning('Robot Error: limit switch hit')
 
     def set_coordinate_system(self, mode):
@@ -371,10 +375,13 @@ def flip_coordinates(self, coordinates, mode='absolute'):
         return coordinates
 
     def wait_for_arrival(self, tolerance=0.1):
-        arrived = False
         coords = self.get_position()
-        while not arrived:
+
+        prev_max_diff = 0
+        did_move_timestamp = time.time()
+        while True:
             self.check_paused_stopped()
+            self.connection.serial_pause()
             coords = self.get_position()
             diff = {}
             for axis in coords.get('target', {}):
@@ -390,12 +397,14 @@ def wait_for_arrival(self, tolerance=0.1):
             the robot's physical resolution is found with:
             1mm / config_steps_per_mm
             """
-            if dist_head < tolerance:
-                if abs(diff['a']) < tolerance and abs(diff['b']) < tolerance:
-                    arrived = True
-            else:
-                arrived = False
-        return arrived
+            max_diff = max(dist_head, abs(diff['a']), abs(diff['b']))
+            if max_diff < tolerance:
+                return
+            if max_diff != prev_max_diff:
+                did_move_timestamp = time.time()
+            prev_max_diff = max_diff
+            if time.time() - did_move_timestamp > 1.0:
+                raise RuntimeError('Expected robot to move, please reconnect')
 
     def home(self, *axis):
         axis_to_home = ''
@@ -413,12 +422,6 @@ def home(self, *axis):
             raise RuntimeWarning(
                 'HOMING ERROR: Check switches are being pressed and connected')
         if res == 'ok':
-            # the axis aren't necessarily set to 0.0
-            # values after homing, so force it
-            pos_args = {}
-            for l in axis_to_home:
-                pos_args[l] = 0
-
             arguments = {
                 'name': 'home',
                 'axis': axis_to_home,
@@ -428,6 +431,11 @@ def home(self, *axis):
                 }
             }
             trace.EventBroker.get_instance().notify(arguments)
+            # the axis aren't necessarily set to 0.0
+            # values after homing, so force it
+            pos_args = {}
+            for l in axis_to_home:
+                pos_args[l] = 0
             return self.set_position(**pos_args)
         else:
             return False

api/opentrons/drivers/smoothie_drivers/v2_0_0/driver.py

@@ -295,8 +295,11 @@ def flip_coordinates(self, coordinates, mode='absolute'):
     def wait_for_arrival(self, tolerance=0.5):
         target = self.get_target_position()
 
+        prev_max_diff = 0
+        did_move_timestamp = time.time()
         while True:
             self.check_paused_stopped()
+            self.connection.serial_pause()
 
             current = self.get_current_position()
             diff = {}
@@ -307,8 +310,15 @@ def wait_for_arrival(self, tolerance=0.5):
             diff_a = abs(diff.get('a', 0))
             diff_b = abs(diff.get('b', 0))
 
-            if max(diff_head, diff_a, diff_b) < tolerance:
+            max_diff = max(diff_head, diff_a, diff_b)
+
+            if max_diff < tolerance:
                 return
+            if max_diff != prev_max_diff:
+                did_move_timestamp = time.time()
+            prev_max_diff = max_diff
+            if time.time() - did_move_timestamp > 1.0:
+                raise RuntimeError('Expected robot to move, please reconnect')
 
     def home(self, *axis):
 

api/opentrons/robot/robot.py

@@ -24,21 +24,21 @@ class InstrumentMosfet(object):
     Provides access to MagBead's MOSFET.
     """
 
-    def __init__(self, driver, mosfet_index):
-        self.motor_driver = driver
+    def __init__(self, this_robot, mosfet_index):
+        self.robot = this_robot
         self.mosfet_index = mosfet_index
 
     def engage(self):
         """
         Engages the MOSFET.
         """
-        self.motor_driver.set_mosfet(self.mosfet_index, True)
+        self.robot._driver.set_mosfet(self.mosfet_index, True)
 
     def disengage(self):
         """
         Disengages the MOSFET.
         """
-        self.motor_driver.set_mosfet(self.mosfet_index, False)
+        self.robot._driver.set_mosfet(self.mosfet_index, False)
 
     def wait(self, seconds):
         """
@@ -49,15 +49,15 @@ def wait(self, seconds):
         seconds : int
             Number of seconds to pause for.
         """
-        self.motor_driver.wait(seconds)
+        self.robot._driver.wait(seconds)
 
 
 class InstrumentMotor(object):
     """
     Provides access to Robot's head motor.
     """
-    def __init__(self, driver, axis):
-        self.motor_driver = driver
+    def __init__(self, this_robot, axis):
+        self.robot = this_robot
         self.axis = axis
 
     def move(self, value, mode='absolute'):
@@ -71,15 +71,15 @@ def move(self, value, mode='absolute'):
         mode : {'absolute', 'relative'}
         """
         kwargs = {self.axis: value}
-        self.motor_driver.move_plunger(
+        self.robot._driver.move_plunger(
             mode=mode, **kwargs
         )
 
     def home(self):
         """
         Home plunger motor.
         """
-        self.motor_driver.home(self.axis)
+        self.robot._driver.home(self.axis)
 
     def wait(self, seconds):
         """
@@ -90,7 +90,7 @@ def wait(self, seconds):
         seconds : int
             Number of seconds to pause for.
         """
-        self.motor_driver.wait(seconds)
+        self.robot._driver.wait(seconds)
 
     def speed(self, rate):
         """
@@ -100,7 +100,7 @@ def speed(self, rate):
         ----------
         rate : int
         """
-        self.motor_driver.set_plunger_speed(rate, self.axis)
+        self.robot._driver.set_plunger_speed(rate, self.axis)
         return self
 
 
@@ -296,7 +296,7 @@ def get_mosfet(self, mosfet_index):
 
         motor_obj = self.INSTRUMENT_DRIVERS_CACHE.get(key)
         if not motor_obj:
-            motor_obj = InstrumentMosfet(self._driver, mosfet_index)
+            motor_obj = InstrumentMosfet(self, mosfet_index)
             self.INSTRUMENT_DRIVERS_CACHE[key] = motor_obj
         return motor_obj
 
@@ -314,7 +314,7 @@ def get_motor(self, axis):
 
         motor_obj = self.INSTRUMENT_DRIVERS_CACHE.get(key)
         if not motor_obj:
-            motor_obj = InstrumentMotor(self._driver, axis)
+            motor_obj = InstrumentMotor(self, axis)
             self.INSTRUMENT_DRIVERS_CACHE[key] = motor_obj
         return motor_obj
 

api/tests/opentrons/drivers/smoothie_drivers/v1_2_0/test_motor.py

@@ -30,6 +30,9 @@ def setUp(self):
 
         self.motor = self.robot._driver
 
+    def test_is_simulating(self):
+        self.assertTrue(self.motor.is_simulating())
+
     def test_reset(self):
         self.motor.reset()
         self.assertFalse(self.motor.is_connected())
@@ -280,6 +283,13 @@ def test_wait_for_arrival(self):
         self.motor.move_head(z=30)
         self.motor.wait_for_arrival()
 
+        old_coords = dict(self.motor.connection.serial_port.coordinates)
+        vs = self.motor.connection.serial_port
+        for ax in vs.coordinates['target'].keys():
+            vs.coordinates['target'][ax] += 10
+        self.assertRaises(RuntimeError, self.motor.wait_for_arrival)
+        vs.coordinates = old_coords
+
     def test_move_relative(self):
         self.motor.home()
         self.motor.move_head(x=100, y=100, z=100)

api/tests/opentrons/drivers/smoothie_drivers/v2_0_0/test_motor.py

@@ -29,6 +29,9 @@ def setUp(self):
 
         self.motor = self.robot._driver
 
+    def test_is_simulating(self):
+        self.assertTrue(self.motor.is_simulating())
+
     def test_reset(self):
         self.motor.reset()
         self.assertFalse(self.motor.is_connected())
@@ -288,6 +291,13 @@ def test_wait_for_arrival(self):
         self.motor.move_head(z=30)
         self.motor.wait_for_arrival()
 
+        old_coords = dict(self.motor.connection.serial_port.coordinates)
+        vs = self.motor.connection.serial_port
+        for ax in vs.coordinates['target'].keys():
+            vs.coordinates['target'][ax] += 10
+        self.assertRaises(RuntimeError, self.motor.wait_for_arrival)
+        vs.coordinates = old_coords
+
     def test_move_relative(self):
         self.motor.home()
         self.motor.move_head(x=100, y=100, z=100)