Add lider ROS node and move robot control into its own directory

robot/raspbot_v2_interface/motor_controller.py

@@ -0,0 +1,109 @@
+from .Raspbot_Lib import Raspbot
+
+# Motor ID assignments (from Raspbot_Lib comments)
+_MOTOR_FL = 0  # L1 — front left
+_MOTOR_RL = 1  # L2 — rear left
+_MOTOR_FR = 2  # R1 — front right
+_MOTOR_RR = 3  # R2 — rear right
+
+_DRIVER_MAX = 255
+
+
+class MotorController:
+    """
+    Thin adapter between the ROS 2 motor controller node and Raspbot_Lib.
+
+    Speed convention: the node passes values in the range
+    [-max_speed, max_speed] (default max_speed = 1.0).  This class scales
+    them linearly to the driver's 0-255 range, so max_speed should match
+    whatever normalised unit you use (1.0 → full throttle).
+    """
+
+    def __init__(self, max_speed: float = 1.0):
+        self._max_speed = max_speed
+        self._bot: Raspbot | None = None
+        # Shadow register — Raspbot_Lib has no read-back, so we track state.
+        self._speeds = [0.0, 0.0, 0.0, 0.0]  # [FL, FR, RL, RR]
+
+    # ------------------------------------------------------------------
+    # Lifecycle
+    # ------------------------------------------------------------------
+
+    def initialize(self) -> None:
+        """Create the I2C connection and ensure all motors are stopped."""
+        self._bot = Raspbot()
+        self._stop_all_motors()
+
+    def shutdown(self) -> None:
+        """Stop motors and release the driver handle."""
+        if self._bot is not None:
+            self._stop_all_motors()
+            self._bot = None
+
+    # ------------------------------------------------------------------
+    # Motor control
+    # ------------------------------------------------------------------
+
+    def set_speed(
+        self,
+        front_left: float,
+        front_right: float,
+        rear_left: float,
+        rear_right: float,
+    ) -> None:
+        """
+        Set all four wheel speeds simultaneously.
+
+        Parameters
+        ----------
+        front_left, front_right, rear_left, rear_right:
+            Desired speeds in the range [-max_speed, max_speed].
+            Positive = forward, negative = backward.
+        """
+        self._require_initialized()
+        mapping = [
+            (_MOTOR_FL, front_left),
+            (_MOTOR_RL, rear_left),
+            (_MOTOR_FR, front_right),
+            (_MOTOR_RR, rear_right),
+        ]
+        for motor_id, speed in mapping:
+            self._bot.Ctrl_Muto(motor_id, self._to_driver_units(speed))
+
+        self._speeds = [front_left, front_right, rear_left, rear_right]
+
+    def stop_all(self) -> None:
+        """Immediately stop all motors."""
+        self._require_initialized()
+        self._stop_all_motors()
+        self._speeds = [0.0, 0.0, 0.0, 0.0]
+
+    def get_speeds(self) -> list[float]:
+        """
+        Return the last commanded wheel speeds as [FL, FR, RL, RR].
+
+        The Raspbot driver provides no encoder/tachometer read-back, so this
+        returns the most recently written values.
+        """
+        return list(self._speeds)
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _to_driver_units(self, speed: float) -> int:
+        """Scale a speed value to the driver's integer range [-255, 255]."""
+        if self._max_speed == 0:
+            return 0
+        scaled = (speed / self._max_speed) * _DRIVER_MAX
+        return int(max(-_DRIVER_MAX, min(_DRIVER_MAX, scaled)))
+
+    def _stop_all_motors(self) -> None:
+        for motor_id in (_MOTOR_FL, _MOTOR_RL, _MOTOR_FR, _MOTOR_RR):
+            self._bot.Ctrl_Muto(motor_id, 0)
+
+    def _require_initialized(self) -> None:
+        if self._bot is None:
+            raise RuntimeError(
+                "MotorController has not been initialized. Call initialize() first."
+            )