From 299f53957cd4f0cfd4a7baa112c61cac95a1a63d Mon Sep 17 00:00:00 2001
From: Reto Trappitsch <reto@galactic-forensics.space>
Date: Tue, 15 Mar 2022 10:58:58 -0400
Subject: [PATCH 1/2] Add backlash correction to motor commands for
 APTMotorController

---
 .../tests/test_thorlabs/test_thorlabs_apt.py  | 59 +++++++++++++++++++
 instruments/thorlabs/thorlabsapt.py           | 51 ++++++++++++++++
 2 files changed, 110 insertions(+)

diff --git a/instruments/tests/test_thorlabs/test_thorlabs_apt.py b/instruments/tests/test_thorlabs/test_thorlabs_apt.py
index d1ed659f..39026360 100644
--- a/instruments/tests/test_thorlabs/test_thorlabs_apt.py
+++ b/instruments/tests/test_thorlabs/test_thorlabs_apt.py
@@ -1174,6 +1174,65 @@ def test_apt_mc_position(init_kdc101):
         )
 
 
+def test_apt_mc_backlash_correction(init_kdc101):
+    """Get / set backlash correction."""
+    with expected_protocol(
+        ik.thorlabs.APTMotorController,
+        [
+            init_kdc101[0],
+            ThorLabsPacket(
+                message_id=ThorLabsCommands.MOT_REQ_GENMOVEPARAMS,
+                param1=0x01,
+                param2=0x00,
+                dest=0x50,
+                source=0x01,
+                data=None,
+            ).pack(),
+            ThorLabsPacket(
+                message_id=ThorLabsCommands.MOT_SET_GENMOVEPARAMS,
+                param1=None,
+                param2=None,
+                dest=0x50,
+                source=0x01,
+                data=struct.pack("<Hl", 0x01, 1000),
+            ).pack(),
+            ThorLabsPacket(
+                message_id=ThorLabsCommands.MOT_SET_GENMOVEPARAMS,
+                param1=None,
+                param2=None,
+                dest=0x50,
+                source=0x01,
+                data=struct.pack("<Hl", 0x01, 1919),
+            ).pack(),
+        ],
+        [
+            init_kdc101[1],
+            ThorLabsPacket(
+                message_id=ThorLabsCommands.MOT_GET_GENMOVEPARAMS,
+                param1=None,
+                param2=None,
+                dest=0x50,
+                source=0x01,
+                data=struct.pack("<Hl", 0x01, -20000),
+            ).pack(),
+        ],
+        sep="",
+    ) as apt:
+        assert (
+            apt.channel[0].backlash_correction
+            == u.Quantity(-20000, "counts") / apt.channel[0].scale_factors[0]
+        )
+        apt.channel[0].backlash_correction = 1000
+
+        # unitful backlash correction
+        apt.channel[0].motor_model = "PRM1-Z8"
+        apt.channel[0].backlash_correction = u.Quantity(1, u.deg)
+
+        # bad units
+        with pytest.raises(ValueError):
+            apt.channel[0].backlash_correction = 10 * u.mm
+
+
 def test_apt_mc_position_encoder(init_kdc101):
     """Get unitful position of encoder, in counts."""
     with expected_protocol(
diff --git a/instruments/thorlabs/thorlabsapt.py b/instruments/thorlabs/thorlabsapt.py
index 3414b098..30821191 100644
--- a/instruments/thorlabs/thorlabsapt.py
+++ b/instruments/thorlabs/thorlabsapt.py
@@ -1267,6 +1267,57 @@ def motor_model(self, newval):
 
         # MOTOR COMMANDS #
 
+        @property
+        def backlash_correction(self):
+            """Get the backlash correction, if stage is defined, unitful.
+
+            :return: Unitful quantity of backlash correction.
+            """
+            pkt = _packets.ThorLabsPacket(
+                message_id=_cmds.ThorLabsCommands.MOT_REQ_GENMOVEPARAMS,
+                param1=self._idx_chan,
+                param2=0x00,
+                dest=self._apt.destination,
+                source=0x01,
+                data=None,
+            )
+            response = self._apt.querypacket(
+                pkt,
+                expect=_cmds.ThorLabsCommands.MOT_GET_GENMOVEPARAMS,
+                expect_data_len=6,
+            )
+            # chan, pos
+            _, pos = struct.unpack("<Hl", response.data)
+            return u.Quantity(pos, "counts") / self.scale_factors[0]
+
+        @backlash_correction.setter
+        def backlash_correction(self, pos):
+            if not isinstance(pos, u.Quantity):
+                pos_ec = int(pos)
+            else:
+                if pos.units == u.counts:
+                    pos_ec = int(pos.magnitude)
+                else:
+                    scaled_pos = pos * self.scale_factors[0]
+                    # Force a unit error.
+                    try:
+                        pos_ec = int(scaled_pos.to(u.counts).magnitude)
+                    except:
+                        raise ValueError(
+                            "Provided units are not compatible "
+                            "with current motor scale factor."
+                        )
+            # create package to send
+            pkt = _packets.ThorLabsPacket(
+                message_id=_cmds.ThorLabsCommands.MOT_SET_GENMOVEPARAMS,
+                param1=None,
+                param2=None,
+                dest=self._apt.destination,
+                source=0x01,
+                data=struct.pack("<Hl", self._idx_chan, pos_ec),
+            )
+            self._apt.sendpacket(pkt)
+
         @property
         def status_bits(self):
             """

From 3be11f688e7c0f4072c775a6025b789e7a70f45c Mon Sep 17 00:00:00 2001
From: Reto Trappitsch <reto@galactic-forensics.space>
Date: Tue, 15 Mar 2022 11:09:03 -0400
Subject: [PATCH 2/2] Extend tests for full coverage

---
 instruments/tests/test_thorlabs/test_thorlabs_apt.py | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/instruments/tests/test_thorlabs/test_thorlabs_apt.py b/instruments/tests/test_thorlabs/test_thorlabs_apt.py
index 39026360..0e377c42 100644
--- a/instruments/tests/test_thorlabs/test_thorlabs_apt.py
+++ b/instruments/tests/test_thorlabs/test_thorlabs_apt.py
@@ -1196,6 +1196,14 @@ def test_apt_mc_backlash_correction(init_kdc101):
                 source=0x01,
                 data=struct.pack("<Hl", 0x01, 1000),
             ).pack(),
+            ThorLabsPacket(
+                message_id=ThorLabsCommands.MOT_SET_GENMOVEPARAMS,
+                param1=None,
+                param2=None,
+                dest=0x50,
+                source=0x01,
+                data=struct.pack("<Hl", 0x01, 1000),
+            ).pack(),
             ThorLabsPacket(
                 message_id=ThorLabsCommands.MOT_SET_GENMOVEPARAMS,
                 param1=None,
@@ -1223,10 +1231,11 @@ def test_apt_mc_backlash_correction(init_kdc101):
             == u.Quantity(-20000, "counts") / apt.channel[0].scale_factors[0]
         )
         apt.channel[0].backlash_correction = 1000
+        apt.channel[0].backlash_correction = 1000 * u.counts
 
         # unitful backlash correction
         apt.channel[0].motor_model = "PRM1-Z8"
-        apt.channel[0].backlash_correction = u.Quantity(1, u.deg)
+        apt.channel[0].backlash_correction = 1 * u.deg
 
         # bad units
         with pytest.raises(ValueError):