jetson/ros2_ws/src/saltybot_rover_driver/config/rover_params.yaml

@@ -0,0 +1,26 @@
+rover_driver:
+  ros__parameters:
+    # Serial interface
+    serial_port: /dev/ttyUSB_rover
+    baud_rate: 115200
+
+    # Drive variant
+    four_wheel: true          # true=4WD skid-steer, false=2WD differential
+
+    # Physical parameters
+    track_width: 0.28         # left↔right wheel centre distance (m)
+    wheelbase: 0.30           # front↔rear axle distance (m) — used for odometry
+
+    # Speed limits
+    max_speed_ms: 2.0         # m/s at full ESC deflection
+    max_linear_vel: 1.5       # clamp on cmd_vel linear.x (m/s)
+    max_angular_vel: 3.0      # clamp on cmd_vel angular.z (rad/s)
+
+    # ESC PWM settings
+    pwm_neutral_us: 1500      # neutral/brake pulse width (µs)
+    pwm_range_us: 500         # half-range from neutral → [1000..2000] µs
+
+    # Timing
+    cmd_vel_timeout_s: 0.5    # deadman: zero ESCs if /cmd_vel goes silent
+    heartbeat_period_s: 0.2   # H\n heartbeat interval
+    control_rate: 50.0        # Hz

jetson/ros2_ws/src/saltybot_rover_driver/launch/rover_driver.launch.py

@@ -0,0 +1,53 @@
+"""
+rover_driver.launch.py — Launch the SaltyRover ESC motor driver node.
+
+Usage:
+    ros2 launch saltybot_rover_driver rover_driver.launch.py
+    ros2 launch saltybot_rover_driver rover_driver.launch.py serial_port:=/dev/ttyACM1
+    ros2 launch saltybot_rover_driver rover_driver.launch.py four_wheel:=false
+"""
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_share = get_package_share_directory("saltybot_rover_driver")
+    params_file = os.path.join(pkg_share, "config", "rover_params.yaml")
+
+    serial_port_arg = DeclareLaunchArgument(
+        "serial_port",
+        default_value="/dev/ttyUSB_rover",
+        description="Serial port for ESC controller",
+    )
+    four_wheel_arg = DeclareLaunchArgument(
+        "four_wheel",
+        default_value="true",
+        description="Enable 4WD skid-steer (true) or 2WD differential (false)",
+    )
+
+    rover_driver = Node(
+        package="saltybot_rover_driver",
+        executable="rover_driver_node",
+        name="rover_driver",
+        parameters=[
+            params_file,
+            {
+                "serial_port": LaunchConfiguration("serial_port"),
+                "four_wheel":  LaunchConfiguration("four_wheel"),
+            },
+        ],
+        output="screen",
+        emulate_tty=True,
+    )
+
+    return LaunchDescription([
+        serial_port_arg,
+        four_wheel_arg,
+        rover_driver,
+    ])

jetson/ros2_ws/src/saltybot_rover_driver/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>saltybot_rover_driver</name>
+  <version>0.1.0</version>
+  <description>SaltyRover 4-wheel independent ESC motor driver (Issue #110)</description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/kinematics.py

@@ -0,0 +1,156 @@
+"""
+kinematics.py — Differential / skid-steer kinematics for SaltyRover (Issue #110).
+
+Supports two drive variants selectable at runtime:
+
+  2-wheel (differential drive)
+  ────────────────────────────
+  Standard unicycle model: one left motor, one right motor.
+
+    v_left  = linear_x - angular_z * track_width / 2
+    v_right = linear_x + angular_z * track_width / 2
+
+  4-wheel (skid steer)
+  ─────────────────────
+  Left-side motors share the same speed; right-side motors share the same speed.
+  The kinematic formula is identical to 2-wheel — the rover simply applies
+  the same left/right speeds to front and rear motors on each side.
+
+    v_left_front  = v_left
+    v_left_rear   = v_left
+    v_right_front = v_right
+    v_right_rear  = v_right
+
+  This gives accurate straight-line and point-turn behaviour.  Skid losses
+  during cornering are handled by the ESC/hardware; the Jetson does not
+  model them.
+
+Output
+──────
+  Wheel speeds are returned in m/s, then clipped to ±max_wheel_speed and
+  converted to ESC PWM duty-cycle (µs) by the caller.
+
+Pure module — no ROS2 dependency; fully unit-testable.
+"""
+
+import math
+
+
+# ─── Wheel-speed computation ────────────────────────────────────────────────
+
+def two_wheel_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+) -> tuple:
+    """
+    Unicycle → differential drive.
+
+    Parameters
+    ----------
+    linear_x   : forward velocity (m/s)
+    angular_z  : yaw rate (rad/s, +ve = CCW / left turn)
+    track_width: centre-to-centre wheel separation (m)
+
+    Returns
+    -------
+    (v_left, v_right) in m/s
+    """
+    half = angular_z * track_width / 2.0
+    return linear_x - half, linear_x + half
+
+
+def four_wheel_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+) -> tuple:
+    """
+    Unicycle → skid-steer 4-wheel drive.
+
+    Returns
+    -------
+    (v_left_front, v_left_rear, v_right_front, v_right_rear) in m/s
+    """
+    v_left, v_right = two_wheel_speeds(linear_x, angular_z, track_width)
+    return v_left, v_left, v_right, v_right
+
+
+# ─── m/s → ESC PWM ─────────────────────────────────────────────────────────
+
+def speed_to_pwm(
+    speed_ms: float,
+    max_speed_ms: float,
+    pwm_neutral_us: int,
+    pwm_range_us: int,
+) -> int:
+    """
+    Convert wheel speed (m/s) to ESC PWM pulse width (µs).
+
+    Linear mapping:
+        pwm = neutral + (speed / max_speed) * range
+
+    Parameters
+    ----------
+    speed_ms      : desired wheel speed (m/s); negative = reverse
+    max_speed_ms  : speed at full deflection (m/s)
+    pwm_neutral_us: centre / brake pulse width (µs), typically 1500
+    pwm_range_us  : half-range from neutral to full (µs), typically 500
+
+    Returns
+    -------
+    PWM pulse width in µs, clamped to [neutral - range, neutral + range].
+    """
+    if max_speed_ms <= 0.0:
+        return pwm_neutral_us
+    normalised = max(-1.0, min(1.0, speed_ms / max_speed_ms))
+    pwm = pwm_neutral_us + normalised * pwm_range_us
+    lo  = pwm_neutral_us - pwm_range_us
+    hi  = pwm_neutral_us + pwm_range_us
+    return int(round(max(lo, min(hi, pwm))))
+
+
+# ─── Variant-aware speed computation ────────────────────────────────────────
+
+def compute_wheel_speeds(
+    linear_x: float,
+    angular_z: float,
+    track_width: float,
+    four_wheel: bool,
+    max_speed_ms: float,
+) -> tuple:
+    """
+    Compute (and clip) wheel speeds for the selected drive variant.
+
+    Returns a tuple of clipped m/s values:
+      2-wheel → (v_left, v_right)
+      4-wheel → (v_left_front, v_left_rear, v_right_front, v_right_rear)
+
+    Clipping is symmetric around zero at ±max_speed_ms.
+    """
+    def clip(v):
+        return max(-max_speed_ms, min(max_speed_ms, v))
+
+    if four_wheel:
+        vfl, vlr, vrf, vrr = four_wheel_speeds(linear_x, angular_z, track_width)
+        return clip(vfl), clip(vlr), clip(vrf), clip(vrr)
+    else:
+        vl, vr = two_wheel_speeds(linear_x, angular_z, track_width)
+        return clip(vl), clip(vr)
+
+
+# ─── Odometry helper ────────────────────────────────────────────────────────
+
+def odometry_from_wheel_speeds(
+    v_left: float,
+    v_right: float,
+    track_width: float,
+) -> tuple:
+    """
+    Estimate linear and angular velocity from left/right wheel speeds.
+
+    Returns (linear_x, angular_z) — the inverse of two_wheel_speeds.
+    """
+    linear_x  = (v_left + v_right) / 2.0
+    angular_z = (v_right - v_left) / track_width if track_width > 0.0 else 0.0
+    return linear_x, angular_z

jetson/ros2_ws/src/saltybot_rover_driver/saltybot_rover_driver/rover_driver_node.py

@@ -0,0 +1,326 @@
+"""
+rover_driver_node.py — SaltyRover 4-wheel independent ESC motor driver (Issue #110).
+
+Hardware
+────────
+  SaltyRover: 4-wheel ground robot with individual brushless ESCs.
+  ESCs controlled via PWM (servo-style 1000–2000 µs pulses).
+  Communication: USB CDC serial to STM32 or Raspberry Pi Pico GPIO PWM bridge.
+
+  ESC channel assignments (configurable):
+    CH1 = left-front
+    CH2 = left-rear
+    CH3 = right-front
+    CH4 = right-rear
+
+Drive variants
+──────────────
+  Selectable at runtime via `four_wheel` parameter (default True).
+
+  2-wheel (four_wheel=False):
+    Left motors share a single speed, right motors share a single speed.
+    Equivalent to standard differential-drive; only CH1+CH3 are used
+    (or all four with mirrored values, depending on hardware).
+
+  4-wheel skid-steer (four_wheel=True):
+    All four ESCs commanded independently. Front/rear same-side share speed,
+    making this kinematically equivalent to 2-wheel but all four motors driven.
+
+Serial protocol
+────────────────
+  Command frame (ASCII, newline-terminated):
+    P<ch1_us>,<ch2_us>,<ch3_us>,<ch4_us>\n
+    Values: 1000–2000 µs (1500 = neutral/brake)
+
+  Heartbeat:
+    H\n  every heartbeat_period_s (ESC controller zeros PWM after timeout)
+
+Subscribes
+──────────
+  /cmd_vel  (geometry_msgs/Twist) — Nav2-compatible velocity command
+              linear.x  = forward (m/s)
+              angular.z = yaw rate (rad/s, +CCW)
+
+Publishes
+─────────
+  /saltybot/rover_pwm   (std_msgs/String JSON) — current PWM values (observability)
+  /saltybot/rover_odom  (nav_msgs/Odometry)    — wheel-speed odometry
+
+Parameters
+──────────
+  serial_port        /dev/ttyUSB_rover  (or /dev/ttyACM1, etc.)
+  baud_rate          115200
+  four_wheel         True   — runtime variant flag (toggle live via ros2 param set)
+  track_width        0.28   — left↔right wheel centre distance (m)
+  max_speed_ms       2.0    — m/s at full ESC deflection
+  max_linear_vel     1.5    — clamp on cmd_vel linear.x (m/s)
+  max_angular_vel    3.0    — clamp on cmd_vel angular.z (rad/s)
+  pwm_neutral_us     1500   — neutral (brake) PWM (µs)
+  pwm_range_us       500    — half-range from neutral (µs) → [1000..2000]
+  cmd_vel_timeout_s  0.5    — deadman: zero ESCs if /cmd_vel goes silent
+  heartbeat_period_s 0.2
+  control_rate       50.0   — Hz
+  wheelbase          0.30   — front↔rear axle distance for odometry (m)
+
+Usage
+─────
+  ros2 launch saltybot_rover_driver rover_driver.launch.py
+  ros2 param set /rover_driver four_wheel false   # switch to 2-wheel live
+"""
+
+import json
+import math
+import threading
+import time
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+
+from geometry_msgs.msg import Twist
+from nav_msgs.msg import Odometry
+from std_msgs.msg import Header, String
+
+from saltybot_rover_driver.kinematics import compute_wheel_speeds, speed_to_pwm
+
+try:
+    import serial
+    _SERIAL_OK = True
+except ImportError:
+    _SERIAL_OK = False
+
+
+class RoverDriverNode(Node):
+
+    def __init__(self):
+        super().__init__("rover_driver")
+
+        # ── Parameters ────────────────────────────────────────────────────────
+        self.declare_parameter("serial_port",         "/dev/ttyUSB_rover")
+        self.declare_parameter("baud_rate",            115200)
+        self.declare_parameter("four_wheel",           True)
+        self.declare_parameter("track_width",          0.28)
+        self.declare_parameter("max_speed_ms",         2.0)
+        self.declare_parameter("max_linear_vel",       1.5)
+        self.declare_parameter("max_angular_vel",      3.0)
+        self.declare_parameter("pwm_neutral_us",       1500)
+        self.declare_parameter("pwm_range_us",         500)
+        self.declare_parameter("cmd_vel_timeout_s",    0.5)
+        self.declare_parameter("heartbeat_period_s",   0.2)
+        self.declare_parameter("control_rate",        50.0)
+        self.declare_parameter("wheelbase",            0.30)
+
+        self._p = self._load_params()
+
+        # ── State ─────────────────────────────────────────────────────────────
+        self._target_linear_x:  float = 0.0
+        self._target_angular_z: float = 0.0
+        self._last_cmd_vel_t:   float = 0.0
+        self._last_pwm:         tuple = (1500, 1500, 1500, 1500)
+        self._odom_x:   float = 0.0
+        self._odom_y:   float = 0.0
+        self._odom_yaw: float = 0.0
+        self._last_odom_t: float = time.monotonic()
+
+        # ── Serial ────────────────────────────────────────────────────────────
+        self._ser = None
+        self._ser_lock = threading.Lock()
+        if _SERIAL_OK:
+            self._open_serial()
+        else:
+            self.get_logger().warn(
+                "pyserial not installed — running in simulation mode (no serial I/O)")
+
+        # ── QoS ───────────────────────────────────────────────────────────────
+        reliable = QoSProfile(
+            reliability=ReliabilityPolicy.RELIABLE,
+            history=HistoryPolicy.KEEP_LAST,
+            depth=10,
+        )
+
+        # ── Subscriptions ─────────────────────────────────────────────────────
+        self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, reliable)
+
+        # ── Publishers ────────────────────────────────────────────────────────
+        self._pwm_pub  = self.create_publisher(String,  "/saltybot/rover_pwm",  reliable)
+        self._odom_pub = self.create_publisher(Odometry, "/saltybot/rover_odom", reliable)
+
+        # ── Timers ────────────────────────────────────────────────────────────
+        rate = self._p["control_rate"]
+        self._ctrl_timer = self.create_timer(1.0 / rate,           self._control_cb)
+        self._hb_timer   = self.create_timer(self._p["hb_period"], self._heartbeat_cb)
+
+        self.get_logger().info(
+            f"RoverDriverNode ready  "
+            f"variant={'4WD' if self._p['four_wheel'] else '2WD'}  "
+            f"track={self._p['track_width']}m  "
+            f"max_speed={self._p['max_speed_ms']}m/s  "
+            f"port={self._p['serial_port']}"
+        )
+
+    # ── Parameter load ────────────────────────────────────────────────────────
+
+    def _load_params(self) -> dict:
+        return {
+            "serial_port":    self.get_parameter("serial_port").value,
+            "baud_rate":      self.get_parameter("baud_rate").value,
+            "four_wheel":     self.get_parameter("four_wheel").value,
+            "track_width":    self.get_parameter("track_width").value,
+            "max_speed_ms":   self.get_parameter("max_speed_ms").value,
+            "max_linear_vel": self.get_parameter("max_linear_vel").value,
+            "max_angular_vel":self.get_parameter("max_angular_vel").value,
+            "pwm_neutral":    int(self.get_parameter("pwm_neutral_us").value),
+            "pwm_range":      int(self.get_parameter("pwm_range_us").value),
+            "cmd_timeout":    self.get_parameter("cmd_vel_timeout_s").value,
+            "hb_period":      self.get_parameter("heartbeat_period_s").value,
+            "control_rate":   self.get_parameter("control_rate").value,
+            "wheelbase":      self.get_parameter("wheelbase").value,
+        }
+
+    # ── Serial helpers ────────────────────────────────────────────────────────
+
+    def _open_serial(self) -> None:
+        with self._ser_lock:
+            try:
+                self._ser = serial.Serial(
+                    self._p["serial_port"], self._p["baud_rate"], timeout=0.05)
+                self.get_logger().info(f"Serial open: {self._p['serial_port']}")
+            except Exception as exc:
+                self.get_logger().error(f"Cannot open serial: {exc}")
+                self._ser = None
+
+    def _write(self, data: bytes) -> bool:
+        with self._ser_lock:
+            if self._ser is None or not self._ser.is_open:
+                return False
+            try:
+                self._ser.write(data)
+                return True
+            except Exception as exc:
+                self.get_logger().error(f"Serial write: {exc}", throttle_duration_sec=2.0)
+                self._ser = None
+                return False
+
+    # ── cmd_vel callback ──────────────────────────────────────────────────────
+
+    def _cmd_vel_cb(self, msg: Twist) -> None:
+        p = self._p
+        self._target_linear_x  = max(-p["max_linear_vel"],
+                                      min(p["max_linear_vel"],  msg.linear.x))
+        self._target_angular_z = max(-p["max_angular_vel"],
+                                      min(p["max_angular_vel"], msg.angular.z))
+        self._last_cmd_vel_t = time.monotonic()
+
+    # ── 50 Hz control loop ────────────────────────────────────────────────────
+
+    def _control_cb(self) -> None:
+        self._p = self._load_params()
+        p   = self._p
+        now = time.monotonic()
+
+        # Deadman
+        cmd_age = (now - self._last_cmd_vel_t) if self._last_cmd_vel_t > 0.0 else 1e9
+        if cmd_age > p["cmd_timeout"]:
+            lin_x, ang_z = 0.0, 0.0
+        else:
+            lin_x, ang_z = self._target_linear_x, self._target_angular_z
+
+        # Kinematics → wheel speeds
+        speeds = compute_wheel_speeds(
+            lin_x, ang_z,
+            track_width=p["track_width"],
+            four_wheel=p["four_wheel"],
+            max_speed_ms=p["max_speed_ms"],
+        )
+
+        # Pad to 4 values for protocol; 2-wheel mirrors front to rear
+        if len(speeds) == 2:
+            vl, vr = speeds
+            speeds4 = (vl, vl, vr, vr)
+        else:
+            speeds4 = speeds   # (vfl, vlr, vrf, vrr)
+
+        # Convert to PWM
+        def to_pwm(v):
+            return speed_to_pwm(v, p["max_speed_ms"], p["pwm_neutral"], p["pwm_range"])
+
+        pwm = tuple(to_pwm(v) for v in speeds4)
+        self._last_pwm = pwm
+
+        # Send P command
+        frame = f"P{pwm[0]},{pwm[1]},{pwm[2]},{pwm[3]}\n".encode("ascii")
+        self._write(frame)
+
+        # Publish observability
+        pwm_msg = String()
+        pwm_msg.data = json.dumps({
+            "ch1_us": pwm[0], "ch2_us": pwm[1],
+            "ch3_us": pwm[2], "ch4_us": pwm[3],
+            "variant": "4WD" if p["four_wheel"] else "2WD",
+        })
+        self._pwm_pub.publish(pwm_msg)
+
+        # Odometry integration (dead reckoning)
+        dt = now - self._last_odom_t
+        self._last_odom_t = now
+        self._integrate_odometry(speeds4[0], speeds4[2], p["track_width"], dt)
+        self._publish_odom(now)
+
+    # ── Heartbeat ─────────────────────────────────────────────────────────────
+
+    def _heartbeat_cb(self) -> None:
+        self._write(b"H\n")
+
+    # ── Odometry ─────────────────────────────────────────────────────────────
+
+    def _integrate_odometry(
+        self, v_left: float, v_right: float, track_width: float, dt: float
+    ) -> None:
+        """Simple 2D dead-reckoning from left/right motor speeds."""
+        lin  = (v_left + v_right) / 2.0
+        ang  = (v_right - v_left) / track_width if track_width > 0.0 else 0.0
+        self._odom_yaw += ang * dt
+        self._odom_x   += lin * math.cos(self._odom_yaw) * dt
+        self._odom_y   += lin * math.sin(self._odom_yaw) * dt
+
+    def _publish_odom(self, now: float) -> None:
+        yaw = self._odom_yaw
+        msg = Odometry()
+        msg.header.stamp    = self.get_clock().now().to_msg()
+        msg.header.frame_id = "odom"
+        msg.child_frame_id  = "base_link"
+
+        msg.pose.pose.position.x = self._odom_x
+        msg.pose.pose.position.y = self._odom_y
+        msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
+        msg.pose.pose.orientation.w = math.cos(yaw / 2.0)
+
+        lin_x = (self._last_pwm[0] + self._last_pwm[2]) / 2.0  # rough proxy
+        msg.twist.twist.linear.x  = self._target_linear_x
+        msg.twist.twist.angular.z = self._target_angular_z
+
+        self._odom_pub.publish(msg)
+
+    # ── Lifecycle ─────────────────────────────────────────────────────────────
+
+    def destroy_node(self) -> None:
+        self._write(b"P1500,1500,1500,1500\n")   # neutral on shutdown
+        super().destroy_node()
+
+
+# ── Entry point ───────────────────────────────────────────────────────────────
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RoverDriverNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_rover_driver/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/saltybot_rover_driver
+
+[install]
+install_scripts=$base/lib/saltybot_rover_driver

jetson/ros2_ws/src/saltybot_rover_driver/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup, find_packages
+import os
+from glob import glob
+
+package_name = "saltybot_rover_driver"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=find_packages(exclude=["test"]),
+    data_files=[
+        ("share/ament_index/resource_index/packages",
+         [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (os.path.join("share", package_name, "config"),
+         glob("config/*.yaml")),
+        (os.path.join("share", package_name, "launch"),
+         glob("launch/*.py")),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description="SaltyRover 4-wheel independent ESC motor driver",
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            f"rover_driver_node = {package_name}.rover_driver_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_rover_driver/test/test_rover_kinematics.py

@@ -0,0 +1,295 @@
+"""
+test_rover_kinematics.py — Unit tests for kinematics.py (Issue #110).
+
+No ROS2 runtime required — pure Python.
+"""
+
+import math
+import pytest
+
+from saltybot_rover_driver.kinematics import (
+    two_wheel_speeds,
+    four_wheel_speeds,
+    speed_to_pwm,
+    compute_wheel_speeds,
+    odometry_from_wheel_speeds,
+)
+
+
+# ─── two_wheel_speeds ────────────────────────────────────────────────────────
+
+class TestTwoWheelSpeeds:
+
+    def test_straight_ahead(self):
+        vl, vr = two_wheel_speeds(1.0, 0.0, 0.28)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_straight_reverse(self):
+        vl, vr = two_wheel_speeds(-1.0, 0.0, 0.28)
+        assert vl == pytest.approx(-1.0)
+        assert vr == pytest.approx(-1.0)
+
+    def test_point_turn_ccw(self):
+        """Pure CCW rotation: linear_x=0, angular_z>0 → left slower, right faster."""
+        vl, vr = two_wheel_speeds(0.0, 1.0, 0.28)
+        assert vl == pytest.approx(-0.14)
+        assert vr == pytest.approx(0.14)
+
+    def test_point_turn_cw(self):
+        """Pure CW rotation: linear_x=0, angular_z<0 → left faster, right slower."""
+        vl, vr = two_wheel_speeds(0.0, -1.0, 0.28)
+        assert vl == pytest.approx(0.14)
+        assert vr == pytest.approx(-0.14)
+
+    def test_forward_left_curve(self):
+        vl, vr = two_wheel_speeds(1.0, 0.5, 0.28)
+        assert vl == pytest.approx(1.0 - 0.5 * 0.28 / 2.0)
+        assert vr == pytest.approx(1.0 + 0.5 * 0.28 / 2.0)
+
+    def test_zero_inputs(self):
+        vl, vr = two_wheel_speeds(0.0, 0.0, 0.28)
+        assert vl == pytest.approx(0.0)
+        assert vr == pytest.approx(0.0)
+
+    def test_zero_track_width(self):
+        """track_width=0 should not crash; both wheels get linear_x."""
+        vl, vr = two_wheel_speeds(1.0, 1.0, 0.0)
+        assert vl == pytest.approx(1.0)
+        assert vr == pytest.approx(1.0)
+
+    def test_symmetry(self):
+        """Reversing angular_z mirrors left/right."""
+        vl1, vr1 = two_wheel_speeds(0.5, 0.3, 0.28)
+        vl2, vr2 = two_wheel_speeds(0.5, -0.3, 0.28)
+        assert vl1 == pytest.approx(vr2)
+        assert vr1 == pytest.approx(vl2)
+
+
+# ─── four_wheel_speeds ───────────────────────────────────────────────────────
+
+class TestFourWheelSpeeds:
+
+    def test_straight(self):
+        vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.0, 0.28)
+        assert vfl == pytest.approx(1.0)
+        assert vlr == pytest.approx(1.0)
+        assert vrf == pytest.approx(1.0)
+        assert vrr == pytest.approx(1.0)
+
+    def test_front_rear_share_speed(self):
+        """Front and rear on each side must have identical speeds."""
+        vfl, vlr, vrf, vrr = four_wheel_speeds(1.0, 0.5, 0.28)
+        assert vfl == pytest.approx(vlr)
+        assert vrf == pytest.approx(vrr)
+
+    def test_point_turn(self):
+        vfl, vlr, vrf, vrr = four_wheel_speeds(0.0, 1.0, 0.28)
+        expected_l = -0.14
+        expected_r = 0.14
+        assert vfl == pytest.approx(expected_l)
+        assert vlr == pytest.approx(expected_l)
+        assert vrf == pytest.approx(expected_r)
+        assert vrr == pytest.approx(expected_r)
+
+    def test_returns_four_values(self):
+        result = four_wheel_speeds(1.0, 0.3, 0.28)
+        assert len(result) == 4
+
+    def test_consistent_with_two_wheel(self):
+        """4-wheel is just 2-wheel spread to four channels."""
+        vl, vr = two_wheel_speeds(0.8, 0.4, 0.30)
+        vfl, vlr, vrf, vrr = four_wheel_speeds(0.8, 0.4, 0.30)
+        assert vfl == pytest.approx(vl)
+        assert vlr == pytest.approx(vl)
+        assert vrf == pytest.approx(vr)
+        assert vrr == pytest.approx(vr)
+
+
+# ─── speed_to_pwm ────────────────────────────────────────────────────────────
+
+class TestSpeedToPwm:
+
+    def test_zero_speed_is_neutral(self):
+        assert speed_to_pwm(0.0, 2.0, 1500, 500) == 1500
+
+    def test_full_forward(self):
+        assert speed_to_pwm(2.0, 2.0, 1500, 500) == 2000
+
+    def test_full_reverse(self):
+        assert speed_to_pwm(-2.0, 2.0, 1500, 500) == 1000
+
+    def test_half_forward(self):
+        assert speed_to_pwm(1.0, 2.0, 1500, 500) == 1750
+
+    def test_half_reverse(self):
+        assert speed_to_pwm(-1.0, 2.0, 1500, 500) == 1250
+
+    def test_over_max_clamps_to_2000(self):
+        assert speed_to_pwm(5.0, 2.0, 1500, 500) == 2000
+
+    def test_under_min_clamps_to_1000(self):
+        assert speed_to_pwm(-5.0, 2.0, 1500, 500) == 1000
+
+    def test_zero_max_speed_returns_neutral(self):
+        assert speed_to_pwm(1.0, 0.0, 1500, 500) == 1500
+
+    def test_negative_max_speed_returns_neutral(self):
+        assert speed_to_pwm(1.0, -1.0, 1500, 500) == 1500
+
+    def test_returns_int(self):
+        result = speed_to_pwm(0.5, 2.0, 1500, 500)
+        assert isinstance(result, int)
+
+    def test_custom_neutral_and_range(self):
+        """Supports non-standard neutral/range values."""
+        pwm = speed_to_pwm(1.0, 2.0, 1400, 400)
+        assert pwm == 1600   # 1400 + 0.5*400
+
+    def test_quarter_speed(self):
+        pwm = speed_to_pwm(0.5, 2.0, 1500, 500)
+        assert pwm == 1625   # 1500 + 0.25*500
+
+    def test_rounding(self):
+        """Result should be integer-rounded, not truncated."""
+        # speed=1/3 of max → 1500 + (1/3)*500 = 1666.67 → rounds to 1667
+        pwm = speed_to_pwm(2.0 / 3.0, 2.0, 1500, 500)
+        assert pwm == 1667
+
+
+# ─── compute_wheel_speeds ────────────────────────────────────────────────────
+
+class TestComputeWheelSpeeds:
+
+    def test_two_wheel_mode_returns_two_values(self):
+        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        assert len(result) == 2
+
+    def test_four_wheel_mode_returns_four_values(self):
+        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
+        assert len(result) == 4
+
+    def test_clipping_two_wheel(self):
+        """Sharp turn at full speed: outer wheel must be clipped to max."""
+        result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        vl, vr = result
+        assert vl >= -2.0
+        assert vr <= 2.0
+
+    def test_clipping_four_wheel(self):
+        result = compute_wheel_speeds(2.0, 5.0, 0.28, four_wheel=True, max_speed_ms=2.0)
+        for v in result:
+            assert -2.0 <= v <= 2.0
+
+    def test_straight_no_clip(self):
+        result = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        assert result == pytest.approx((1.0, 1.0))
+
+    def test_four_wheel_symmetric(self):
+        vfl, vlr, vrf, vrr = compute_wheel_speeds(0.5, 0.2, 0.28, four_wheel=True, max_speed_ms=2.0)
+        assert vfl == pytest.approx(vlr)
+        assert vrf == pytest.approx(vrr)
+
+    def test_zero_inputs_two_wheel(self):
+        result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        assert result == pytest.approx((0.0, 0.0))
+
+    def test_zero_inputs_four_wheel(self):
+        result = compute_wheel_speeds(0.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
+        assert result == pytest.approx((0.0, 0.0, 0.0, 0.0))
+
+    def test_negative_speed_two_wheel(self):
+        vl, vr = compute_wheel_speeds(-1.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        assert vl == pytest.approx(-1.0)
+        assert vr == pytest.approx(-1.0)
+
+
+# ─── odometry_from_wheel_speeds ──────────────────────────────────────────────
+
+class TestOdometryFromWheelSpeeds:
+
+    def test_straight(self):
+        lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.28)
+        assert lin == pytest.approx(1.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_point_turn_ccw(self):
+        """Equal and opposite: left=-v, right=+v → angular = 2v/track."""
+        v = 0.14
+        lin, ang = odometry_from_wheel_speeds(-v, v, 0.28)
+        assert lin == pytest.approx(0.0)
+        assert ang == pytest.approx(1.0)
+
+    def test_point_turn_cw(self):
+        v = 0.14
+        lin, ang = odometry_from_wheel_speeds(v, -v, 0.28)
+        assert lin == pytest.approx(0.0)
+        assert ang == pytest.approx(-1.0)
+
+    def test_zero_inputs(self):
+        lin, ang = odometry_from_wheel_speeds(0.0, 0.0, 0.28)
+        assert lin == pytest.approx(0.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_zero_track_width_no_crash(self):
+        lin, ang = odometry_from_wheel_speeds(1.0, 1.0, 0.0)
+        assert lin == pytest.approx(1.0)
+        assert ang == pytest.approx(0.0)
+
+    def test_inverse_of_two_wheel_speeds(self):
+        """odometry_from_wheel_speeds should be the inverse of two_wheel_speeds."""
+        lin_in, ang_in = 0.7, 0.5
+        track = 0.28
+        vl, vr = two_wheel_speeds(lin_in, ang_in, track)
+        lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track)
+        assert lin_out == pytest.approx(lin_in)
+        assert ang_out == pytest.approx(ang_in)
+
+    def test_reverse(self):
+        lin, ang = odometry_from_wheel_speeds(-1.0, -1.0, 0.28)
+        assert lin == pytest.approx(-1.0)
+        assert ang == pytest.approx(0.0)
+
+
+# ─── Integration: kinematics round-trip ─────────────────────────────────────
+
+class TestRoundTrip:
+
+    @pytest.mark.parametrize("lin,ang", [
+        (1.0,  0.0),
+        (0.0,  1.0),
+        (0.5,  0.5),
+        (-0.5, 0.3),
+        (1.0, -0.8),
+    ])
+    def test_roundtrip_two_wheel(self, lin, ang):
+        """compute → odometry_from → same (lin, ang) for unclipped inputs."""
+        track = 0.28
+        vl, vr = compute_wheel_speeds(lin, ang, track, four_wheel=False, max_speed_ms=2.0)
+        lin_out, ang_out = odometry_from_wheel_speeds(vl, vr, track)
+        assert lin_out == pytest.approx(lin, abs=1e-9)
+        assert ang_out == pytest.approx(ang, abs=1e-9)
+
+    def test_pwm_neutral_at_zero_speed(self):
+        pwm = speed_to_pwm(0.0, 2.0, 1500, 500)
+        assert pwm == 1500
+
+    def test_pwm_full_deflection_forward(self):
+        vl, vr = compute_wheel_speeds(2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        pwm_l = speed_to_pwm(vl, 2.0, 1500, 500)
+        pwm_r = speed_to_pwm(vr, 2.0, 1500, 500)
+        assert pwm_l == 2000
+        assert pwm_r == 2000
+
+    def test_pwm_full_deflection_reverse(self):
+        vl, vr = compute_wheel_speeds(-2.0, 0.0, 0.28, four_wheel=False, max_speed_ms=2.0)
+        pwm_l = speed_to_pwm(vl, 2.0, 1500, 500)
+        pwm_r = speed_to_pwm(vr, 2.0, 1500, 500)
+        assert pwm_l == 1000
+        assert pwm_r == 1000
+
+    def test_four_wheel_all_channels_driven(self):
+        """All four PWMs should be non-neutral during forward motion."""
+        vfl, vlr, vrf, vrr = compute_wheel_speeds(1.0, 0.0, 0.28, four_wheel=True, max_speed_ms=2.0)
+        pwms = [speed_to_pwm(v, 2.0, 1500, 500) for v in (vfl, vlr, vrf, vrr)]
+        assert all(p > 1500 for p in pwms)