update gear motor position handling after move

api/src/opentrons/config/defaults_ot3.py

@@ -86,7 +86,7 @@
  OT3AxisKind.Z: 35,
  OT3AxisKind.P: 15,
  OT3AxisKind.Z_G: 50,
- OT3AxisKind.Q: 5.5,
+ OT3AxisKind.Q: 10,
  },
  low_throughput={
  OT3AxisKind.X: 350,

api/src/opentrons/hardware_control/backends/ot3controller.py

@@ -80,7 +80,6 @@
 from opentrons_hardware.hardware_control.motor_position_status import (
  get_motor_position,
  update_motor_position_estimation,
- update_gear_motor_position_estimation,
 )
 from opentrons_hardware.hardware_control.limit_switches import get_limit_switches
 from opentrons_hardware.hardware_control.tip_presence import get_tip_ejector_state
@@ -361,13 +360,6 @@ async def update_motor_status(self) -> None:
  response = await get_motor_position(self._messenger, motor_nodes)
  self._handle_motor_status_response(response)
 
- async def update_gear_motor_position(self) -> None:
- motor_response = await update_gear_motor_position_estimation(self._messenger)
- updated_successfully = motor_response[1]
- # update position only if updated successfully by the firmware
- if updated_successfully:
- self._gear_motor_position = {NodeId.pipette_left: motor_response[0]}
-
  async def update_motor_estimation(self, axes: Sequence[Axis]) -> None:
  """Update motor position estimation for commanded nodes, and update cache of data."""
  nodes = set([axis_to_node(a) for a in axes])
@@ -653,7 +645,6 @@ async def tip_action(
  self._gear_motor_position = {
  NodeId.pipette_left: positions[NodeId.pipette_left][0]
  }
- print(f"setting gear motor position to {positions[NodeId.pipette_left][0]}")
 
  @requires_update
  async def gripper_grip_jaw(

api/src/opentrons/hardware_control/backends/ot3utils.py

@@ -56,6 +56,8 @@
 )
 from opentrons_hardware.hardware_control.constants import interrupts_per_sec
 
+from opentrons_hardware.hardware_control.constants import interrupts_per_sec
+
 GRIPPER_JAW_HOME_TIME: float = 10
 GRIPPER_JAW_GRIP_TIME: float = 10
 
@@ -406,6 +408,8 @@ def create_tip_action_group(
  for move in moves:
  unit_vector = move.unit_vector
  for block in move.blocks:
+ if block.time < (3.0 / interrupts_per_sec):
+ continue
  velocities = unit_vector_multiplication(unit_vector, block.initial_speed)
  accelerations = unit_vector_multiplication(unit_vector, block.acceleration)
  step = create_tip_action_step(

api/src/opentrons/hardware_control/ot3api.py

@@ -811,8 +811,6 @@ async def refresh_positions(self) -> None:
  await self._backend.update_motor_status()
  await self._cache_current_position()
  await self._cache_encoder_position()
- if self._gantry_load == GantryLoad.HIGH_THROUGHPUT:
- await self._backend.update_gear_motor_position()
 
  async def _cache_current_position(self) -> Dict[Axis, float]:
  """Cache current position from backend and return in absolute deck coords."""
@@ -1684,30 +1682,35 @@ async def _motor_pick_up_tip(
  pipette_spec.tiprack_down,
  self._current_position,
  )
+ # check if position is known before pick up tip
  await self._move(target_down)
- # perform pick up tip
- await self._backend.update_gear_motor_position()
+ homing_velocity = self._config.motion_settings.max_speed_discontinuity[
+ GantryLoad.HIGH_THROUGHPUT
+ ][OT3AxisKind.Q]
+ if not any(self._backend.gear_motor_position):
+ # home gear motor if position not known
+ await self._backend.tip_action(
+ distance=self._backend.axis_bounds[Axis.Q][1],
+ velocity=homing_velocity,
+ tip_action="home",
+ )
  pipette_axis = Axis.of_main_tool_actuator(mount)
  gear_origin_float = axis_convert(self._backend.gear_motor_position, 0.0)[
  pipette_axis
  ]
- gear_origin_dict = {Axis.Q: gear_origin_float}
  clamp_move_target = pipette_spec.pick_up_distance
- gear_target_dict = {Axis.Q: clamp_move_target}
- clamp_moves = self._build_moves(gear_origin_dict, gear_target_dict)
+ clamp_moves = self._build_moves(
+ {Axis.Q: gear_origin_float}, {Axis.Q: clamp_move_target}
+ )
  await self._backend.tip_action(moves=clamp_moves[0], tip_action="clamp")
 
- homing_velocity = self._config.motion_settings.max_speed_discontinuity[
- GantryLoad.HIGH_THROUGHPUT
- ][OT3AxisKind.Q]
-
  gear_pos_float = axis_convert(self._backend.gear_motor_position, 0.0)[
  Axis.P_L
  ]
- gear_pos_dict = {Axis.Q: gear_pos_float}
- fast_home_target = {Axis.Q: self._config.safe_home_distance}
 
- fast_home_moves = self._build_moves(gear_pos_dict, fast_home_target)
+ fast_home_moves = self._build_moves(
+ {Axis.Q: gear_pos_float}, {Axis.Q: self._config.safe_home_distance}
+ )
  # move toward home until a safe distance
  await self._backend.tip_action(moves=fast_home_moves[0], tip_action="clamp")
  # move the rest of the way home with no acceleration
@@ -1781,32 +1784,38 @@ async def drop_tip(
  realmount = OT3Mount.from_mount(mount)
  spec, _remove = self._pipette_handler.plan_check_drop_tip(realmount, home_after)
 
+ homing_velocity = self._config.motion_settings.max_speed_discontinuity[
+ GantryLoad.HIGH_THROUGHPUT
+ ][OT3AxisKind.Q]
  for move in spec.drop_moves:
  await self._backend.set_active_current(move.current)
 
  if move.is_ht_tip_action and move.speed:
  # The speed check is needed because speed can sometimes be None.
  # Not sure why
- await self._backend.update_gear_motor_position()
+ if not any(self._backend.gear_motor_position):
+ # home gear motor if position not known
+ await self._backend.tip_action(
+ distance=self._backend.axis_bounds[Axis.Q][1],
+ velocity=homing_velocity,
+ tip_action="home",
+ )
+
  gear_start_position = axis_convert(
  self._backend.gear_motor_position, 0.0
+ )[Axis.P_L]
+ drop_moves = self._build_moves(
+ {Axis.Q: gear_start_position}, {Axis.Q: move.target_position}
  )
- pickup_target_dict = {Axis.Q: move.target_position}
- drop_moves = self._build_moves(gear_start_position, pickup_target_dict)
-
  await self._backend.tip_action(moves=drop_moves[0], tip_action="clamp")
 
- homing_velocity = self._config.motion_settings.max_speed_discontinuity[
- GantryLoad.HIGH_THROUGHPUT
- ][OT3AxisKind.Q]
-
  gear_pos_float = axis_convert(self._backend.gear_motor_position, 0.0)[
  Axis.P_L
  ]
- gear_pos_dict = {Axis.Q: gear_pos_float}
- fast_home_target = {Axis.Q: self._config.safe_home_distance}
 
- fast_home_moves = self._build_moves(gear_pos_dict, fast_home_target)
+ fast_home_moves = self._build_moves(
+ {Axis.Q: gear_pos_float}, {Axis.Q: self._config.safe_home_distance}
+ )
  # move toward home until a safe distance
  await self._backend.tip_action(
  moves=fast_home_moves[0], tip_action="clamp"

api/tests/opentrons/hardware_control/test_ot3_api.py

@@ -1359,7 +1359,7 @@ def _fake_function():
  with patch.object(
  backend, "tip_action", AsyncMock(spec=backend.tip_action)
  ) as tip_action:
-
+ backend._gear_motor_position = {NodeId: 0}
  pipette_handler.plan_check_pick_up_tip.return_value = (
  PickUpTipSpec(
  plunger_prep_pos=0,
@@ -1428,6 +1428,7 @@ def _fake_function():
  with patch.object(
  backend, "tip_action", AsyncMock(spec=backend.tip_action)
  ) as tip_action:
+ backend._gear_motor_position = {NodeId.pipette_left: 0}
  pipette_handler.plan_check_drop_tip.return_value = (
  DropTipSpec(
  drop_moves=[
@@ -1466,7 +1467,6 @@ def _update_gear_motor_pos(
  await ot3_hardware.set_gantry_load(GantryLoad.HIGH_THROUGHPUT)
  await ot3_hardware.drop_tip(Mount.LEFT, home_after=True)
  pipette_handler.plan_check_drop_tip.assert_called_once_with(OT3Mount.LEFT, True)
-
  # first call should be "clamp", moving down
  assert tip_action.call_args_list[0][-1]["tip_action"] == "clamp"
  assert tip_action.call_args_list[0][-1]["moves"][0].unit_vector == {Axis.Q: 1}

hardware-testing/hardware_testing/opentrons_api/helpers_ot3.py

@@ -458,7 +458,7 @@ async def move_tip_motor_relative_ot3(
  if speed is not None and distance < 0:
  speed *= -1
 
- tip_motor_move = api._build_moves(current_gear_pos_dict, target_pos_dict, speed)
+ tip_motor_move = api._build_moves(current_gear_pos_dict, target_pos_dict)
 
  _move_coro = api._backend.tip_action(
  moves=tip_motor_move[0],

hardware/opentrons_hardware/hardware_control/motion_planning/types.py

@@ -163,18 +163,18 @@ def dummy_value() -> np.float64:
  max_speed=np.float64(0),
  blocks=(
  Block(
- distance=np.float64(0),
- initial_speed=np.float64(0),
+ distance=np.float64(20),
+ initial_speed=np.float64(3),
  acceleration=np.float64(0),
  ),
  Block(
- distance=np.float64(0),
- initial_speed=np.float64(0),
+ distance=np.float64(20),
+ initial_speed=np.float64(3),
  acceleration=np.float64(0),
  ),
  Block(
- distance=np.float64(0),
- initial_speed=np.float64(0),
+ distance=np.float64(20),
+ initial_speed=np.float64(3),
  acceleration=np.float64(0),
  ),
  ),

hardware/opentrons_hardware/hardware_control/motor_position_status.py

@@ -17,8 +17,6 @@
  MotorPositionResponse,
  UpdateMotorPositionEstimationRequest,
  UpdateMotorPositionEstimationResponse,
- UpdateGearMotorPositionEstimationRequest,
- UpdateGearMotorPositionEstimationResponse,
 )
 from opentrons_hardware.firmware_bindings.arbitration_id import ArbitrationId
 from opentrons_hardware.firmware_bindings.constants import (
@@ -100,7 +98,7 @@ async def _parser_update_motor_position_response(
  node = NodeId(arb_id.parts.originating_node_id)
  if node == expected:
  return (
- float(response.payload.current_position.value) / 1000.0,
+ float(response.payload.current_position.value / 1000.0),
  float(response.payload.encoder_position.value) / 1000.0,
  bool(
  response.payload.position_flags.value
@@ -161,62 +159,3 @@ def _listener_filter(arbitration_id: ArbitrationId) -> bool:
  )
 
  return data
-
-
-async def update_gear_motor_position_estimation(
- can_messenger: CanMessenger, timeout: float = 1.0
-) -> Tuple[float, bool]:
- """Updates the estimation of motor position on selected nodes.
-
- Request node to update motor position from its encoder and respond
- with updated motor and encoder status.
- """
-
- def _listener_filter(arbitration_id: ArbitrationId) -> bool:
- return (
- NodeId(arbitration_id.parts.originating_node_id) in {NodeId.pipette_left}
- ) and (
- MessageId(arbitration_id.parts.message_id)
- in [
- UpdateGearMotorPositionEstimationResponse.message_id,
- ]
- )
-
- data = []
-
- with MultipleMessagesWaitableCallback(
- can_messenger,
- _listener_filter,
- 2,
- ) as reader:
- await can_messenger.send(
- node_id=NodeId.pipette_left,
- message=UpdateGearMotorPositionEstimationRequest(),
- )
- try:
- response = await asyncio.wait_for(
- _parser_update_gear_motor_position_response(
- reader, NodeId.pipette_left
- ),
- timeout,
- )
- data.append(response)
- except asyncio.TimeoutError:
- log.warning("Update motor position estimation timed out")
- return 0, False
-
- return data[0]
-
-
-async def _parser_update_gear_motor_position_response(
- reader: WaitableCallback, expected: NodeId
-) -> Tuple[float, bool]:
- async for response, arb_id in reader:
- if isinstance(response, UpdateGearMotorPositionEstimationResponse):
- node = NodeId(arb_id.parts.originating_node_id)
- if node == expected:
- return (
- float(response.payload.current_position_um.value / 1000.0),
- bool(MotorPositionFlags.stepper_position_ok.value),
- )
- raise StopAsyncIteration

hardware/opentrons_hardware/hardware_control/move_group_runner.py

@@ -173,21 +173,31 @@ def _accumulate_move_completions(
  position: NodeDict[
  List[Tuple[Tuple[int, int], float, float, bool, bool]]
  ] = defaultdict(list)
+ gear_motor_position: NodeDict[
+ List[Tuple[Tuple[int, int], float, float, bool, bool]]
+ ] = defaultdict(list)
  for arbid, completion in completions:
  if isinstance(completion, TipActionResponse):
- gear_position_response = (
- float(completion.payload.current_position_um.value) / 1000.0,
- float(completion.payload.encoder_position_um.value) / 1000.0,
- bool(
- completion.payload.position_flags.value
- & MotorPositionFlags.stepper_position_ok.value
- ),
- bool(
- completion.payload.position_flags.value
- & MotorPositionFlags.encoder_position_ok.value
- ),
+ # if any completions are TipActionResponses, separate them from the 'positions'
+ # dict so the left pipette's position doesn't get overwritten
+ gear_motor_position[NodeId(arbid.parts.originating_node_id)].append(
+ (
+ (
+ completion.payload.group_id.value,
+ completion.payload.seq_id.value,
+ ),
+ float(completion.payload.current_position_um.value) / 1000.0,
+ float(completion.payload.encoder_position_um.value) / 1000.0,
+ bool(
+ completion.payload.position_flags.value
+ & MotorPositionFlags.stepper_position_ok.value
+ ),
+ bool(
+ completion.payload.position_flags.value
+ & MotorPositionFlags.encoder_position_ok.value
+ ),
+ )
  )
- return {arbid.parts.originating_node_id: gear_position_response}
  else:
  position[NodeId(arbid.parts.originating_node_id)].append(
  (
@@ -209,6 +219,17 @@ def _accumulate_move_completions(
  )
  # for each node, pull the position from the completion with the largest
  # combination of group id and sequence id
+ if any(gear_motor_position):
+ return {
+ node: next(
+ reversed(
+ sorted(
+ poslist, key=lambda position_element: position_element[0]
+ )
+ )
+ )[1:]
+ for node, poslist in gear_motor_position.items()
+ }
  return {
  node: next(
  reversed(