Implement driving to absolute positions

Using a linear motion profile should be sufficient for now.

schunk_egu_egk_gripper_dummy/src/dummy.py

@@ -4,6 +4,36 @@
 from pathlib import Path
 import json
 import struct
+from typing import Tuple
+
+
+class LinearMotion(object):
+    def __init__(
+        self,
+        initial_pos: float,
+        initial_speed: float,
+        target_pos: float,
+        target_speed: float,
+    ):
+        self.min_speed = 0.001
+        self.initial_pos = initial_pos
+        self.initial_speed = max(0.0, initial_speed)
+        self.target_pos = target_pos
+        self.target_speed = max(self.min_speed, target_speed)
+        self.time_finish = abs(self.target_pos / self.target_speed)
+
+    def sample(self, t: float) -> Tuple[float, float]:
+        if t <= 0.0:
+            return (self.initial_pos, self.initial_speed)
+        if t >= self.time_finish:
+            return (self.target_pos, 0.0)
+
+        v = self.target_speed
+        if self.target_pos < self.initial_pos:
+            v = -v
+        current_pos = v * t + self.initial_pos
+        current_speed = abs(v)
+        return (current_pos, current_speed)
 
 
 class Dummy(object):
@@ -60,6 +90,22 @@ def _run(self) -> None:
             time.sleep(1)
         print("Done")
 
+    def move(self, target_pos: float, target_speed: float) -> None:
+        motion = LinearMotion(
+            initial_pos=self.get_actual_position(),
+            initial_speed=self.get_actual_speed(),
+            target_pos=target_pos,
+            target_speed=target_speed,
+        )
+        start = time.time()
+        actual_pos, actual_speed = motion.sample(0)
+        while abs(actual_pos) < abs(target_pos):
+            t = time.time() - start
+            actual_pos, actual_speed = motion.sample(t)
+            self.set_actual_position(actual_pos)
+            self.set_actual_speed(actual_speed)
+            time.sleep(0.01)
+
     def post(self, msg: dict) -> dict:
         if msg["inst"] == self.plc_output:
             self.plc_output_buffer = bytearray(bytes.fromhex(msg["value"]))
@@ -191,6 +237,14 @@ def set_actual_position(self, position: float) -> None:
     def set_actual_speed(self, speed: float) -> None:
         self.data[self.actual_speed] = [bytes(struct.pack("f", speed)).hex().upper()]
 
+    def get_actual_position(self) -> float:
+        read_pos = self.data[self.actual_position][0]
+        return struct.unpack("f", bytes.fromhex(read_pos))[0]
+
+    def get_actual_speed(self) -> float:
+        read_speed = self.data[self.actual_speed][0]
+        return struct.unpack("f", bytes.fromhex(read_speed))[0]
+
     def process_control_bits(self) -> None:
 
         # Acknowledge
@@ -199,3 +253,13 @@ def process_control_bits(self) -> None:
             self.set_status_bit(bit=7, value=False)
             self.set_status_error("00")
             self.set_status_diagnostics("00")
+
+        # Move to absolute position
+        if self.get_control_bit(13) == 1:
+            self.toggle_status_bit(5)
+            self.move(
+                target_pos=self.get_target_position(),
+                target_speed=self.get_target_speed(),
+            )
+            self.set_status_bit(bit=13, value=True)
+            self.set_status_bit(bit=4, value=True)

schunk_egu_egk_gripper_dummy/tests/test_dummy.py

@@ -1,4 +1,6 @@
 from src.dummy import Dummy
+import pytest
+import struct
 
 # [1]: https://stb.cloud.schunk.com/media/IM0046706.PDF
 
@@ -55,3 +57,29 @@ def test_dummy_is_ready_after_acknowledge():
     assert dummy.get_status_bit(7) == 0  # no error
     assert dummy.get_status_error() == "0"
     assert dummy.get_status_diagnostics() == "0"
+
+
+def test_dummy_moves_to_absolute_position():
+    dummy = Dummy()
+    target_pos = 12.345
+    target_speed = 50.3
+
+    control_double_word = "00200000"
+    set_position = bytes(struct.pack("f", target_pos)).hex().upper()
+    set_speed = bytes(struct.pack("f", target_speed)).hex().upper()
+    gripping_force = "00000000"
+    command = {
+        "inst": dummy.plc_output,
+        "value": control_double_word + set_position + set_speed + gripping_force,
+    }
+    before = dummy.get_status_bit(bit=5)  # command received toggle
+
+    # Motion
+    dummy.post(command)
+
+    # Done
+    assert pytest.approx(dummy.get_actual_position()) == target_pos
+    after = dummy.get_status_bit(bit=5)
+    assert after != before
+    assert dummy.get_status_bit(bit=13) == 1  # position reached
+    assert dummy.get_status_bit(bit=4) == 1  # command successfully processed

schunk_egu_egk_gripper_dummy/tests/test_plc_communication.py

@@ -142,3 +142,17 @@ def test_dummy_supports_writing_actual_speed():
     read_speed = dummy.data[dummy.actual_speed][0]
     read_speed = struct.unpack("f", bytes.fromhex(read_speed))[0]
     assert pytest.approx(read_speed) == speed
+
+
+def test_dummy_supports_reading_actual_position():
+    dummy = Dummy()
+    pos = 0.123
+    dummy.set_actual_position(pos)
+    assert pytest.approx(dummy.get_actual_position()) == pos
+
+
+def test_dummy_supports_reading_actual_speed():
+    dummy = Dummy()
+    speed = 66.5
+    dummy.set_actual_speed(speed)
+    assert pytest.approx(dummy.get_actual_speed()) == speed