diff --git a/sonication_station/jubilee_controller.py b/sonication_station/jubilee_controller.py index 69d90a5..3b27208 100755 --- a/sonication_station/jubilee_controller.py +++ b/sonication_station/jubilee_controller.py @@ -33,8 +33,6 @@ class JubileeMotionController(Inpromptu): TIMEOUT_S = 15 # a general timeout - EPSILON = 0.01 - def __init__(self, debug=False, simulated=False, reset=False): """Start with sane defaults. Setup command and subscribe connections.""" @@ -42,6 +40,7 @@ def __init__(self, debug=False, simulated=False, reset=False): self.debug = debug self.simulated = simulated self.machine_model = {} + self.model_update_timestamp = 0 self.command_socket = None self.wake_time = None # Next scheduled time that the update thread updates. self.state_update_thread = None # The thread. @@ -129,6 +128,7 @@ def parse_string_buffer_into_dicts(packet_buffer_str): with Lock(): # Lock access to the machine model before updating it. while packets: self.machine_model.update(packets.pop(0)) + self.model_update_timestamp = time.perf_counter() # Do scheduled updates on a loop. while self.keep_subscribing: #print(f"thread woke up at {time.perf_counter()}") @@ -143,6 +143,7 @@ def parse_string_buffer_into_dicts(packet_buffer_str): packet_buffer_str = subscribe_socket.recv(self.__class__.MM_BUFFER_SIZE).decode() except json.decoder.JSONDecodeError: print("Buffer too small!") + self.model_update_timestamp = time.perf_counter() packets = parse_string_buffer_into_dicts(packet_buffer_str) with Lock(): # Lock access to the machine model. # TODO: we need a recursive update here for Patch mode. @@ -252,23 +253,32 @@ def home_in_place(self, *args: str): @machine_is_homed - def _move_xyz(self, x: float = None, y: float = None, z: float = None, - wait: bool = True): - """Move in XYZ. Absolute/relative set externally. Optional: wait until done.""" + def _move_xyz(self, x: float = None, y: float = None, z: float = None): + """Move in XYZ. Absolute/relative set externally. Wait until done.""" # TODO: find way to recover from out-of-bounds move requests. - # TODO: check if machine is homed first. Bail early if True. - # TODO: Either start in a state where the machine is not busy so we don't - # save the wrong position and accumulate incorrectly OR - # track the overall commanded position and compare. + # Assume we are starting from an idle state (since no other commands can make the machine busy). + old_x, old_y, old_z = self.position + new_position = (x if x else old_x, y if y else old_y, z if z else old_z) + if not self.absolute_moves: + new_position = (x+old_x if x else old_x, + y+old_y if y else old_y, + z+old_z if z else old_z) + + x_movement = f"X{x} " if x is not None else "" y_movement = f"Y{y} " if y is not None else "" z_movement = f"Z{z} " if z is not None else "" self.gcode(f"G0 {x_movement}{y_movement}{z_movement}F13000") - if not wait: - return - self.wait_until_idle() + # Handle small errors + EPS = 0.001 + curr_position = self.position + while abs(new_position[0] - curr_position[0]) > EPS or \ + abs(new_position[1] - curr_position[1]) > EPS or \ + abs(new_position[2] - curr_position[2]) > EPS: + self._sleep_until_next_update() + curr_position = self.position def _set_absolute_moves(self, force: bool = False): @@ -285,20 +295,18 @@ def _set_relative_moves(self, force: bool = False): self.absolute_moves = False - def move_xyz_relative(self, x: float = None, y: float = None, - z: float = None, wait: bool = True): + def move_xyz_relative(self, x: float = None, y: float = None, z: float = None): """Do a relative move in XYZ.""" self._set_relative_moves() - self._move_xyz(x, y, z, wait=wait) + self._move_xyz(x, y, z) @cli_method - def move_xyz_absolute(self, x: float = None, y: float = None, - z: float = None, wait: bool = True): + def move_xyz_absolute(self, x: float = None, y: float = None, z: float = None): """Do an absolute move in XYZ.""" # TODO: use push and pop sematics instead. self._set_absolute_moves() - self._move_xyz(x, y, z, wait) + self._move_xyz(x, y, z) @property @@ -396,17 +404,17 @@ def keyboard_control(self, prompt: str = "=== Manual Control ==="): key = stdscr.getch() stdscr.refresh() if key == curses.KEY_UP: - self.move_xyz_relative(y=-step_size, wait=False) + self.move_xyz_relative(y=-step_size) elif key == curses.KEY_DOWN: - self.move_xyz_relative(y=step_size, wait=False) + self.move_xyz_relative(y=step_size) elif key == curses.KEY_LEFT: - self.move_xyz_relative(x=step_size, wait=False) + self.move_xyz_relative(x=step_size) elif key == curses.KEY_RIGHT: - self.move_xyz_relative(x=-step_size, wait=False) + self.move_xyz_relative(x=-step_size) elif key == ord('w'): - self.move_xyz_relative(z=step_size, wait=False) + self.move_xyz_relative(z=step_size) elif key == ord('s'): - self.move_xyz_relative(z=-step_size, wait=False) + self.move_xyz_relative(z=-step_size) elif key == ord('['): step_size = step_size/2.0 if step_size < min_step_size: @@ -432,24 +440,30 @@ def wait_until_idle(self, timeout = TIMEOUT_S): # new data after the move command was sent. # Note: we are assuming that if a gcode is acknowledged it immediately # changes from idle to busy if it was not already busy. - self._sleep_until_next_update() + #print(f"start state: {self.machine_model['state']['status'].lower()} | {time.perf_counter():.3f}") self._sleep_until_next_update() while self.is_busy: + #print(f"curr state: {self.machine_model['state']['status'].lower()} | {time.perf_counter():.3f}") if time.perf_counter() - start_wait_time > timeout: - raise RuntimeError("Error: Machine has timed out while waiting " - "for a move to complete.") + raise RuntimeError("Error: Machine has timed out while waiting for a move to complete.") self._sleep_until_next_update() + #print(f"end state: {self.machine_model['state']['status'].lower()} | {time.perf_counter():.3f}") + def _sleep_until_next_update(self): """Sleep until we know the machine model has received fresh data.""" + last_model_update = self.model_update_timestamp + # Sleep at least until the thread is scheduled to update again. #print(f"attempting to sleep at {time.perf_counter()}. Will sleep till {self.wake_time}") sleep_interval = self.wake_time - time.perf_counter() if sleep_interval < 0: # Woke up before or during update thread's execution. Sleep again. #print(f" Awoke too early. Will actually sleep till {self.wake_time + self.__class__.POLL_INTERVAL_S}") sleep_interval = self.wake_time + self.__class__.POLL_INTERVAL_S - time.perf_counter() - # Small delta to guarantee we wakeup after the thread. time.sleep(sleep_interval) + # Sleep in small increments until the thread has finished the current update. + while self.model_update_timestamp == last_model_update: + time.sleep(0.001) def disconnect(self): diff --git a/sonication_station/sonication_station.py b/sonication_station/sonication_station.py index 8a79628..fea4a23 100755 --- a/sonication_station/sonication_station.py +++ b/sonication_station/sonication_station.py @@ -173,12 +173,11 @@ def safe_z(self, z: float = None): @cli_method - def move_xy_absolute(self, x: float = None, y: float = None, - wait: bool = True): + def move_xy_absolute(self, x: float = None, y: float = None): """Move in XY, but include the safe Z retract first if defined.""" if self.safe_z is not None: - super().move_xyz_absolute(z=self.safe_z, wait=wait) - super().move_xyz_absolute(x,y,wait=wait) + super().move_xyz_absolute(z=self.safe_z) + super().move_xyz_absolute(x,y) @cli_method def park_tool(self): @@ -424,11 +423,12 @@ def sonicate_well(self, deck_index: int, row_letter: str, column_index: int, row_index = ord(row_letter.upper()) - 65 # map row letters to numbers. x,y = self._get_well_position(deck_index, row_index, column_index) - print(f"Sonicating at: ({x:.3f}, {y:.3f})") + print(f"Moving to: ({x:.3f}, {y:.3f})") self.move_xy_absolute(x,y) # Position over the well at safe z height. - self.move_xyz_absolute(z=plunge_height, wait=True) - print(f"sonicating for {seconds} seconds!!") + self.move_xyz_absolute(z=plunge_height) + print(f"Sonicating for {seconds} seconds!!") self.sonicator.sonicate(seconds) + print("done!") self.move_xy_absolute() # leave the machine at the safe height. if clean: self.clean_sonicator()