From d65147ec8a68354227e26d1f714e1a4ddb138d43 Mon Sep 17 00:00:00 2001
From: Andy Sigler <andrewsigler1@gmail.com>
Date: Tue, 28 Jun 2022 09:30:31 -0400
Subject: [PATCH] fix(api): smoothie-driver backlash compensation allows all
 axes to move simultaneously (#10923)

* apply backlash correction only after all axes have arrived at target position

* remove overcomplicated plunger-split change

* update plunger test

* lint
---
 .../drivers/smoothie_drivers/driver_3_0.py    | 22 ++++++++++++++-----
 .../integration/test_smoothie.py              |  4 ++--
 2 files changed, 19 insertions(+), 7 deletions(-)

diff --git a/api/src/opentrons/drivers/smoothie_drivers/driver_3_0.py b/api/src/opentrons/drivers/smoothie_drivers/driver_3_0.py
index 9d4386f006a..58c8106af76 100755
--- a/api/src/opentrons/drivers/smoothie_drivers/driver_3_0.py
+++ b/api/src/opentrons/drivers/smoothie_drivers/driver_3_0.py
@@ -1270,11 +1270,21 @@ def create_coords_list(coords_dict: Dict[str, float]) -> CommandBuilder:
             log.info(f"No axes move in {target} from position {self.position}")
             return
 
-        backlash_target = {
-            axis: value + PLUNGER_BACKLASH_MM
+        # Multi-axis movements should include the added backlash.
+        # After all axes arrive at target, finally then apply
+        # a backlash correction to just the plunger axes
+        plunger_backlash_axes = [
+            axis
             for axis, value in target.items()
             if axis in "BC" and self.position[axis] < value
-        }
+        ]
+        backlash_target = {ax: moving_target[ax] for ax in plunger_backlash_axes}
+        moving_target.update(
+            {
+                ax: moving_target[ax] + PLUNGER_BACKLASH_MM
+                for ax in plunger_backlash_axes
+            }
+        )
 
         # whatever else we do to our motion target, if nothing moves in the
         # input we will not command it to move
@@ -1295,7 +1305,7 @@ def build_split(here: float, dest: float, split_distance: float) -> float:
         split_target = {
             ax: build_split(
                 self.position[ax],
-                backlash_target.get(ax, moving_target[ax]),
+                moving_target[ax],
                 split.split_distance,
             )
             for ax, split in self._move_split_config.items()
@@ -1365,12 +1375,14 @@ def build_split(here: float, dest: float, split_distance: float) -> float:
         # introduce the standard currents
         command.add_builder(builder=self._generate_current_command())
 
+        # move to target position, including any added backlash to B/C axes
+        command.add_gcode(GCODE.MOVE).add_builder(builder=primary_command_string)
         if backlash_command_string:
+            # correct the B/C positions
             command.add_gcode(gcode=GCODE.MOVE).add_builder(
                 builder=backlash_command_string
             )
 
-        command.add_gcode(GCODE.MOVE).add_builder(builder=primary_command_string)
         if checked_speed != self._combined_speed:
             command.add_builder(builder=self._build_speed_command(self._combined_speed))
 
diff --git a/api/tests/opentrons/hardware_control/integration/test_smoothie.py b/api/tests/opentrons/hardware_control/integration/test_smoothie.py
index 85b25298966..a160b7c0301 100755
--- a/api/tests/opentrons/hardware_control/integration/test_smoothie.py
+++ b/api/tests/opentrons/hardware_control/integration/test_smoothie.py
@@ -147,8 +147,8 @@ async def test_plunger_commands(subject: SmoothieDriver, spy: MagicMock):
     )
     expected = [
         # Set active axes high
-        "M907 A0.8 B0.05 C0.05 X1.25 Y1.25 Z0.8 G4 P0.005 G0 B4.55 G0 A3.5"
-        " B4.25 C5.55 X10.988 Y2.123 Z2.5",
+        "M907 A0.8 B0.05 C0.05 X1.25 Y1.25 Z0.8 G4 P0.005"
+        " G0 A3.5 B4.55 C5.55 X10.988 Y2.123 Z2.5 G0 B4.25",
         "M400",
         # Set plunger current low
         "M907 A0.8 B0.05 C0.05 X1.25 Y1.25 Z0.8 G4 P0.005",