jetson/ros2_ws/src/saltybot_pid_autotune/config/autotune_config.yaml

@@ -0,0 +1,18 @@
+# PID auto-tuner configuration (Astrom-Hagglund relay method)
+
+pid_autotune:
+  ros__parameters:
+    # Relay magnitude for oscillation (m/s for balance control)
+    relay_magnitude: 0.3
+
+    # Safety: abort if robot tilt exceeds this angle (degrees)
+    tilt_safety_limit: 25.0
+
+    # Maximum duration for entire tuning process (seconds)
+    max_tune_duration: 120.0
+
+    # Number of complete oscillation cycles to measure
+    oscillation_count: 5
+
+    # Time to allow system to settle before starting relay (seconds)
+    settle_time: 2.0

jetson/ros2_ws/src/saltybot_pid_autotune/launch/pid_autotune.launch.py

@@ -0,0 +1,36 @@
+"""Launch file for pid_autotune_node."""
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def generate_launch_description():
+    """Generate launch description for PID auto-tuner."""
+    # Package directory
+    pkg_dir = get_package_share_directory("saltybot_pid_autotune")
+
+    # Parameters
+    config_file = os.path.join(pkg_dir, "config", "autotune_config.yaml")
+
+    # Declare launch arguments
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument(
+                "config_file",
+                default_value=config_file,
+                description="Path to configuration YAML file",
+            ),
+            # PID auto-tuner node
+            Node(
+                package="saltybot_pid_autotune",
+                executable="pid_autotune_node",
+                name="pid_autotune",
+                output="screen",
+                parameters=[LaunchConfiguration("config_file")],
+            ),
+        ]
+    )

jetson/ros2_ws/src/saltybot_pid_autotune/package.xml

@@ -0,0 +1,30 @@
+<?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_pid_autotune</name>
+  <version>0.1.0</version>
+  <description>
+    PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method.
+    Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate
+    period (Tu), then computes Ziegler-Nichols PD gains. Safety abort on >25deg tilt.
+  </description>
+  <maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

jetson/ros2_ws/src/saltybot_pid_autotune/saltybot_pid_autotune/pid_autotune_node.py

@@ -0,0 +1,335 @@
+#!/usr/bin/env python3
+"""PID auto-tuner for SaltyBot using Astrom-Hagglund relay feedback method.
+
+Service-triggered relay oscillation measures ultimate gain (Ku) and ultimate period
+(Tu), then computes Ziegler-Nichols PD gains. Aborts on >25deg tilt for safety.
+
+Service:
+  /saltybot/autotune_pid (std_srvs/Trigger) - Start auto-tuning process
+
+Published topics:
+  /saltybot/autotune_info (std_msgs/String) - JSON with tuning progress/results
+  /saltybot/autotune_cmd_vel (geometry_msgs/Twist) - Relay control during tuning
+
+Subscribed topics:
+  /imu/data (sensor_msgs/Imu) - IMU for tilt safety check
+  /saltybot/balance_feedback (std_msgs/Float32) - Balance angle feedback for oscillation
+"""
+
+import json
+import math
+import time
+from enum import Enum
+from typing import Optional, Tuple, List
+
+import rclpy
+from rclpy.node import Node
+from rclpy.service import Service
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Float32, String
+from std_srvs.srv import Trigger
+
+
+class TuneState(Enum):
+    """State machine for auto-tuning process."""
+    IDLE = 0
+    TUNING = 1
+    MEASURING = 2
+    COMPLETE = 3
+    ABORTED = 4
+
+
+class PIDAutotuneNode(Node):
+    """ROS2 node for automatic PID tuning via relay feedback."""
+
+    def __init__(self):
+        super().__init__("pid_autotune")
+
+        # Parameters
+        self.declare_parameter("relay_magnitude", 0.3)  # Relay on-off magnitude
+        self.declare_parameter("tilt_safety_limit", 25.0)  # degrees
+        self.declare_parameter("max_tune_duration", 120.0)  # seconds
+        self.declare_parameter("oscillation_count", 5)  # Number of cycles to measure
+        self.declare_parameter("settle_time", 2.0)  # Settle before starting relay
+
+        self.relay_magnitude = self.get_parameter("relay_magnitude").value
+        self.tilt_safety_limit = self.get_parameter("tilt_safety_limit").value
+        self.max_tune_duration = self.get_parameter("max_tune_duration").value
+        self.oscillation_count = self.get_parameter("oscillation_count").value
+        self.settle_time = self.get_parameter("settle_time").value
+
+        # State tracking
+        self.tune_state = TuneState.IDLE
+        self.last_imu_tilt = 0.0
+        self.last_feedback = 0.0
+        self.feedback_history: List[Tuple[float, float]] = []  # (time, feedback)
+        self.peaks: List[Tuple[float, float]] = []  # (time, peak_value)
+        self.start_time = None
+
+        # Subscriptions
+        self.create_subscription(Imu, "/imu/data", self._on_imu, 10)
+        self.create_subscription(
+            Float32, "/saltybot/balance_feedback", self._on_feedback, 10
+        )
+
+        # Publications
+        self.pub_cmd_vel = self.create_publisher(Twist, "/saltybot/autotune_cmd_vel", 10)
+        self.pub_info = self.create_publisher(String, "/saltybot/autotune_info", 10)
+
+        # Service
+        self.srv_autotune = self.create_service(
+            Trigger, "/saltybot/autotune_pid", self._handle_autotune_request
+        )
+
+        self.get_logger().info(
+            "PID auto-tuner initialized. Service: /saltybot/autotune_pid\n"
+            f"Relay magnitude: {self.relay_magnitude}, "
+            f"Tilt safety limit: {self.tilt_safety_limit}°, "
+            f"Max duration: {self.max_tune_duration}s"
+        )
+
+    def _on_imu(self, msg: Imu) -> None:
+        """Extract tilt angle from IMU."""
+        # Approximate tilt from acceleration (pitch angle)
+        # pitch = atan2(ax, sqrt(ay^2 + az^2))
+        accel = msg.linear_acceleration
+        self.last_imu_tilt = math.degrees(
+            math.atan2(accel.x, math.sqrt(accel.y**2 + accel.z**2))
+        )
+
+    def _on_feedback(self, msg: Float32) -> None:
+        """Record balance feedback for oscillation measurement."""
+        now = self.get_clock().now().nanoseconds / 1e9
+        self.last_feedback = msg.data
+
+        # During tuning, record history
+        if self.tune_state in (TuneState.MEASURING,):
+            self.feedback_history.append((now, msg.data))
+
+    def _handle_autotune_request(
+        self, request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        """Handle service request to start auto-tuning."""
+        if self.tune_state != TuneState.IDLE:
+            response.success = False
+            response.message = f"Already tuning (state: {self.tune_state.name})"
+            return response
+
+        self.get_logger().info("Starting PID auto-tune...")
+        self.tune_state = TuneState.TUNING
+        self.feedback_history = []
+        self.peaks = []
+        self.start_time = self.get_clock().now().nanoseconds / 1e9
+
+        # Start tuning process
+        self._run_autotuning()
+
+        response.success = self.tune_state == TuneState.COMPLETE
+        response.message = f"Tuning {self.tune_state.name}"
+        return response
+
+    def _run_autotuning(self) -> None:
+        """Execute the complete auto-tuning sequence."""
+        # Phase 1: Settle
+        self.get_logger().info("Phase 1: Settling...")
+        if not self._settle():
+            self.tune_state = TuneState.ABORTED
+            self._publish_info(
+                {"status": "aborted", "reason": "Failed to settle before tuning"}
+            )
+            return
+
+        # Phase 2: Relay oscillation
+        self.get_logger().info("Phase 2: Starting relay oscillation...")
+        self.tune_state = TuneState.MEASURING
+        if not self._relay_oscillation():
+            self.tune_state = TuneState.ABORTED
+            self._publish_info(
+                {"status": "aborted", "reason": "Safety limit exceeded or timeout"}
+            )
+            return
+
+        # Phase 3: Analysis
+        self.get_logger().info("Phase 3: Analyzing measurements...")
+        ku, tu = self._analyze_oscillation()
+        if ku is None or tu is None:
+            self.tune_state = TuneState.ABORTED
+            self._publish_info(
+                {"status": "aborted", "reason": "Insufficient oscillation data"}
+            )
+            return
+
+        # Phase 4: Compute gains
+        kp, kd = self._compute_ziegler_nichols(ku, tu)
+
+        # Done
+        self.tune_state = TuneState.COMPLETE
+        self._publish_info(
+            {
+                "status": "complete",
+                "ku": ku,
+                "tu": tu,
+                "kp": kp,
+                "kd": kd,
+            }
+        )
+        self.get_logger().info(
+            f"Auto-tune complete: Ku={ku:.4f}, Tu={tu:.2f}s, Kp={kp:.4f}, Kd={kd:.4f}"
+        )
+
+    def _settle(self) -> bool:
+        """Allow system to settle before starting relay."""
+        settle_start = self.get_clock().now().nanoseconds / 1e9
+        relay_off = Twist()  # Zero command
+
+        while True:
+            now = self.get_clock().now().nanoseconds / 1e9
+            elapsed = now - settle_start
+
+            # Safety check
+            if abs(self.last_imu_tilt) > self.tilt_safety_limit:
+                self.get_logger().error(
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                )
+                return False
+
+            # Timeout check
+            if elapsed > self.max_tune_duration:
+                self.get_logger().error("Settling timeout")
+                return False
+
+            # Continue settling
+            if elapsed > self.settle_time:
+                return True
+
+            self.pub_cmd_vel.publish(relay_off)
+            time.sleep(0.01)
+
+    def _relay_oscillation(self) -> bool:
+        """Apply relay control to induce oscillation."""
+        relay_on = Twist()
+        relay_on.linear.x = self.relay_magnitude
+
+        relay_off = Twist()
+
+        oscillation_start = self.get_clock().now().nanoseconds / 1e9
+        relay_state = True  # True = relay ON, False = relay OFF
+        last_switch = oscillation_start
+        cycles_measured = 0
+        last_peak_value = None
+
+        while cycles_measured < self.oscillation_count:
+            now = self.get_clock().now().nanoseconds / 1e9
+            elapsed = now - oscillation_start
+
+            # Safety checks
+            if abs(self.last_imu_tilt) > self.tilt_safety_limit:
+                self.get_logger().error(
+                    f"Safety abort: Tilt {self.last_imu_tilt:.1f}° > {self.tilt_safety_limit}°"
+                )
+                return False
+
+            if elapsed > self.max_tune_duration:
+                self.get_logger().error("Relay oscillation timeout")
+                return False
+
+            # Switch relay at zero crossings
+            if relay_state and self.last_feedback < 0:
+                relay_state = False
+                self.peaks.append((now, self.last_feedback))
+                cycles_measured += 1
+                last_switch = now
+                self.get_logger().debug(
+                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                )
+
+            elif not relay_state and self.last_feedback > 0:
+                relay_state = True
+                self.peaks.append((now, self.last_feedback))
+                cycles_measured += 1
+                last_switch = now
+                self.get_logger().debug(
+                    f"Cycle {cycles_measured}: Peak at {now - oscillation_start:.2f}s = {self.last_feedback:.4f}"
+                )
+
+            # Publish relay command
+            cmd = relay_on if relay_state else relay_off
+            self.pub_cmd_vel.publish(cmd)
+
+            # Report progress
+            self._publish_info(
+                {
+                    "status": "measuring",
+                    "cycles": cycles_measured,
+                    "feedback": self.last_feedback,
+                    "tilt": self.last_imu_tilt,
+                }
+            )
+
+            time.sleep(0.01)
+
+        # Stop relay
+        self.pub_cmd_vel.publish(relay_off)
+        return len(self.peaks) >= self.oscillation_count
+
+    def _analyze_oscillation(self) -> Tuple[Optional[float], Optional[float]]:
+        """Calculate Ku and Tu from measured peaks."""
+        if len(self.peaks) < 3:
+            self.get_logger().error(f"Insufficient peaks: {len(self.peaks)}")
+            return None, None
+
+        # Ku = 4*relay_magnitude / (pi * |peak|)
+        peak_values = [abs(p[1]) for p in self.peaks]
+        avg_peak = sum(peak_values) / len(peak_values)
+        ku = (4.0 * self.relay_magnitude) / (math.pi * avg_peak)
+
+        # Tu = 2 * (time between peaks) - average period
+        peak_times = [p[0] for p in self.peaks]
+        time_diffs = [
+            peak_times[i + 1] - peak_times[i] for i in range(len(peak_times) - 1)
+        ]
+        tu = 2.0 * (sum(time_diffs) / len(time_diffs))
+
+        self.get_logger().info(
+            f"Analysis: avg_peak={avg_peak:.4f}, Ku={ku:.4f}, Tu={tu:.2f}s"
+        )
+        return ku, tu
+
+    @staticmethod
+    def _compute_ziegler_nichols(ku: float, tu: float) -> Tuple[float, float]:
+        """Compute Ziegler-Nichols PD gains.
+
+        For PD controller:
+          Kp = 0.6 * Ku
+          Kd = 0.075 * Ku * Tu (approximately Kp*Tu/8)
+        """
+        kp = 0.6 * ku
+        kd = 0.075 * ku * tu
+        return kp, kd
+
+    def _publish_info(self, data: dict) -> None:
+        """Publish tuning information as JSON."""
+        info = {
+            "timestamp": self.get_clock().now().nanoseconds / 1e9,
+            "state": self.tune_state.name,
+            **data,
+        }
+        msg = String(data=json.dumps(info))
+        self.pub_info.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = PIDAutotuneNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

jetson/ros2_ws/src/saltybot_pid_autotune/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/saltybot_pid_autotune
+[egg_info]
+tag_date = 0

jetson/ros2_ws/src/saltybot_pid_autotune/setup.py

@@ -0,0 +1,29 @@
+from setuptools import setup
+
+package_name = "saltybot_pid_autotune"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+        (f"share/{package_name}/launch", ["launch/pid_autotune.launch.py"]),
+        (f"share/{package_name}/config", ["config/autotune_config.yaml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="sl-controls",
+    maintainer_email="sl-controls@saltylab.local",
+    description=(
+        "PID auto-tuner: Astrom-Hagglund relay oscillation with Ziegler-Nichols gains"
+    ),
+    license="MIT",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "pid_autotune_node = saltybot_pid_autotune.pid_autotune_node:main",
+        ],
+    },
+)

jetson/ros2_ws/src/saltybot_pid_autotune/test/test_pid_autotune.py

@@ -0,0 +1,314 @@
+"""Unit tests for pid_autotune_node."""
+
+import pytest
+import json
+import math
+from geometry_msgs.msg import Twist
+from sensor_msgs.msg import Imu
+from std_msgs.msg import Float32, String
+
+import rclpy
+
+# Import the node under test
+from saltybot_pid_autotune.pid_autotune_node import (
+    PIDAutotuneNode,
+    TuneState,
+)
+
+
+@pytest.fixture
+def rclpy_fixture():
+    """Initialize and cleanup rclpy."""
+    rclpy.init()
+    yield
+    rclpy.shutdown()
+
+
+@pytest.fixture
+def node(rclpy_fixture):
+    """Create a PID autotune node instance."""
+    node = PIDAutotuneNode()
+    yield node
+    node.destroy_node()
+
+
+class TestPIDAutotuneNode:
+    """Test suite for PIDAutotuneNode."""
+
+    def test_node_initialization(self, node):
+        """Test that node initializes with correct defaults."""
+        assert node.tune_state == TuneState.IDLE
+        assert node.relay_magnitude == 0.3
+        assert node.tilt_safety_limit == 25.0
+        assert node.max_tune_duration == 120.0
+        assert node.oscillation_count == 5
+        assert node.last_imu_tilt == 0.0
+        assert node.last_feedback == 0.0
+        assert len(node.feedback_history) == 0
+        assert len(node.peaks) == 0
+
+    def test_imu_tilt_extraction(self, node):
+        """Test IMU tilt angle extraction."""
+        msg = Imu()
+        # ax=1, ay=0, az=0 -> pitch = atan2(1, 0) = 90 degrees
+        msg.linear_acceleration.x = 1.0
+        msg.linear_acceleration.y = 0.0
+        msg.linear_acceleration.z = 0.0
+
+        node._on_imu(msg)
+        assert abs(node.last_imu_tilt - 90.0) < 1.0
+
+    def test_imu_small_tilt(self, node):
+        """Test small tilt angle extraction."""
+        msg = Imu()
+        # Simulate 10 degree tilt
+        accel_x = math.sin(math.radians(10))
+        accel_z = math.cos(math.radians(10))
+        msg.linear_acceleration.x = accel_x
+        msg.linear_acceleration.y = 0.0
+        msg.linear_acceleration.z = accel_z
+
+        node._on_imu(msg)
+        assert abs(node.last_imu_tilt - 10.0) < 1.0
+
+    def test_feedback_recording(self, node):
+        """Test feedback value recording."""
+        msg = Float32(data=0.5)
+        node._on_feedback(msg)
+        assert node.last_feedback == 0.5
+
+    def test_feedback_history_during_tuning(self, node):
+        """Test that feedback history is only recorded during MEASURING state."""
+        # Not tuning yet
+        node._on_feedback(Float32(data=1.0))
+        assert len(node.feedback_history) == 0
+
+        # Start measuring
+        node.tune_state = TuneState.MEASURING
+        node.start_time = node.get_clock().now().nanoseconds / 1e9
+
+        node._on_feedback(Float32(data=1.0))
+        assert len(node.feedback_history) == 1
+
+        node._on_feedback(Float32(data=2.0))
+        assert len(node.feedback_history) == 2
+
+    def test_ziegler_nichols_pd_gains(self, node):
+        """Test Ziegler-Nichols PD gain calculation."""
+        # Example: Ku=2.0, Tu=1.0
+        kp, kd = node._compute_ziegler_nichols(2.0, 1.0)
+
+        # Kp = 0.6 * Ku = 0.6 * 2.0 = 1.2
+        assert abs(kp - 1.2) < 1e-6
+
+        # Kd = 0.075 * Ku * Tu = 0.075 * 2.0 * 1.0 = 0.15
+        assert abs(kd - 0.15) < 1e-6
+
+    def test_ziegler_nichols_high_gains(self, node):
+        """Test Z-N gains with higher Ku/Tu values."""
+        kp, kd = node._compute_ziegler_nichols(3.0, 2.0)
+
+        assert abs(kp - 1.8) < 1e-6  # 0.6 * 3.0
+        assert abs(kd - 0.45) < 1e-6  # 0.075 * 3.0 * 2.0
+
+    def test_ultimate_gain_from_relay(self, node):
+        """Test Ku calculation from relay magnitude and peak."""
+        # Ku = 4*relay_magnitude / (pi * |peak|)
+        relay_mag = 0.3
+        peak_amplitude = 0.5
+
+        ku = (4.0 * relay_mag) / (math.pi * peak_amplitude)
+
+        expected = (4.0 * 0.3) / (math.pi * 0.5)
+        assert abs(ku - expected) < 1e-6
+
+    def test_analyze_oscillation_insufficient_peaks(self, node):
+        """Test that analysis fails with insufficient peaks."""
+        node.peaks = [(0.0, 0.5), (1.0, -0.5)]  # Only 2 peaks
+
+        ku, tu = node._analyze_oscillation()
+        assert ku is None
+        assert tu is None
+
+    def test_analyze_oscillation_valid_data(self, node):
+        """Test oscillation analysis with valid peak data."""
+        node.relay_magnitude = 0.2
+        # Simulate 5 alternating peaks every ~1 second
+        node.peaks = [
+            (0.0, -0.4),  # Negative peak
+            (1.0, 0.4),   # Positive peak
+            (2.0, -0.4),  # Negative peak
+            (3.0, 0.4),   # Positive peak
+            (4.0, -0.4),  # Negative peak
+        ]
+
+        ku, tu = node._analyze_oscillation()
+
+        # Should compute meaningful values
+        assert ku is not None
+        assert tu is not None
+        assert ku > 0
+        assert tu > 0
+
+    def test_tune_state_machine(self, node):
+        """Test state machine transitions."""
+        assert node.tune_state == TuneState.IDLE
+
+        # Invalid state for tuning (should reject)
+        node.tune_state = TuneState.TUNING
+        response = node._handle_autotune_request(
+            type("Request", (), {}), type("Response", (), {})
+        )
+        # Should fail because already tuning
+        # (actual test depends on response structure)
+
+    def test_info_json_format(self, node):
+        """Test that info JSON is valid."""
+        info = {
+            "status": "measuring",
+            "cycles": 3,
+            "feedback": 0.5,
+            "tilt": 5.0,
+        }
+
+        json_str = json.dumps(info)
+        parsed = json.loads(json_str)
+
+        assert parsed["status"] == "measuring"
+        assert parsed["cycles"] == 3
+        assert abs(parsed["feedback"] - 0.5) < 1e-6
+        assert abs(parsed["tilt"] - 5.0) < 1e-6
+
+    def test_tilt_safety_limit_check(self, node):
+        """Test that tilt exceeding limit is detected."""
+        node.last_imu_tilt = 30.0  # Exceeds 25 degree limit
+        node.tilt_safety_limit = 25.0
+
+        # In actual settle/relay code, this would trigger abort
+        assert abs(node.last_imu_tilt) > node.tilt_safety_limit
+
+    def test_tilt_safe_margin(self, node):
+        """Test normal tilt well within limits."""
+        node.last_imu_tilt = 10.0
+        node.tilt_safety_limit = 25.0
+
+        assert abs(node.last_imu_tilt) < node.tilt_safety_limit
+
+
+class TestPIDAutotuneScenarios:
+    """Integration-style tests for realistic scenarios."""
+
+    def test_scenario_clean_oscillation(self, node):
+        """Scenario: perfect sinusoidal oscillation."""
+        node.relay_magnitude = 0.25
+
+        # Simulate clean oscillation: 5 peaks, 1Hz frequency
+        node.peaks = [
+            (0.0, -0.5),
+            (1.0, 0.5),
+            (2.0, -0.5),
+            (3.0, 0.5),
+            (4.0, -0.5),
+        ]
+
+        ku, tu = node._analyze_oscillation()
+
+        assert ku is not None
+        assert tu is not None
+        # Tu should be ~2 seconds (period of oscillation)
+        assert 1.8 < tu < 2.2
+
+    def test_scenario_damped_oscillation(self, node):
+        """Scenario: damped oscillation with decaying peaks."""
+        node.relay_magnitude = 0.2
+
+        # Peaks decay over time
+        node.peaks = [
+            (0.0, -0.6),
+            (1.0, 0.55),
+            (2.0, -0.5),
+            (3.0, 0.45),
+            (4.0, -0.4),
+        ]
+
+        ku, tu = node._analyze_oscillation()
+
+        # Should still work with average of peaks
+        assert ku is not None
+        assert tu is not None
+
+    def test_scenario_high_frequency_oscillation(self, node):
+        """Scenario: fast oscillation (0.2Hz)."""
+        node.relay_magnitude = 0.3
+
+        # 5 second period = 0.2 Hz
+        node.peaks = [
+            (0.0, -0.5),
+            (5.0, 0.5),
+            (10.0, -0.5),
+            (15.0, 0.5),
+            (20.0, -0.5),
+        ]
+
+        ku, tu = node._analyze_oscillation()
+
+        assert tu is not None
+        # Period should be ~10 seconds (5 second half-period)
+        assert 9.8 < tu < 10.2
+
+    def test_scenario_z_n_aggressive_tuning(self, node):
+        """Scenario: Z-N gains for aggressive tuning."""
+        # High ultimate gain
+        kp, kd = node._compute_ziegler_nichols(ku=4.0, tu=0.5)
+
+        assert kp == pytest.approx(2.4)  # 0.6 * 4.0
+        assert kd == pytest.approx(0.15)  # 0.075 * 4.0 * 0.5
+
+    def test_scenario_z_n_conservative_tuning(self, node):
+        """Scenario: Z-N gains for conservative tuning."""
+        # Low ultimate gain
+        kp, kd = node._compute_ziegler_nichols(ku=1.0, tu=2.0)
+
+        assert kp == pytest.approx(0.6)  # 0.6 * 1.0
+        assert kd == pytest.approx(0.15)  # 0.075 * 1.0 * 2.0
+
+    def test_scenario_tilt_safety_aborts(self, node):
+        """Scenario: tuning aborted due to tilt exceeding limit."""
+        node.last_imu_tilt = 28.0  # Exceeds 25 degree limit
+        node.tilt_safety_limit = 25.0
+
+        # Safety check would catch this
+        should_abort = abs(node.last_imu_tilt) > node.tilt_safety_limit
+        assert should_abort is True
+
+    def test_scenario_normal_balance_tuning(self, node):
+        """Scenario: typical balance tuning process."""
+        # Starting state
+        assert node.tune_state == TuneState.IDLE
+
+        # After settle
+        node.tune_state = TuneState.TUNING
+        assert node.tune_state == TuneState.TUNING
+
+        # After relay measurement
+        node.tune_state = TuneState.MEASURING
+        node.peaks = [
+            (0.0, -0.3),
+            (1.5, 0.3),
+            (3.0, -0.3),
+            (4.5, 0.3),
+            (6.0, -0.3),
+        ]
+
+        ku, tu = node._analyze_oscillation()
+        assert ku is not None
+        assert tu is not None
+
+        # Compute gains
+        kp, kd = node._compute_ziegler_nichols(ku, tu)
+        assert kp > 0
+        assert kd > 0
+
+        # Complete
+        node.tune_state = TuneState.COMPLETE
+        assert node.tune_state == TuneState.COMPLETE

ui/social-bot/src/App.jsx

@@ -50,6 +50,9 @@ import { CameraViewer } from './components/CameraViewer.jsx';
 // Event log (issue #192)
 import { EventLog } from './components/EventLog.jsx';
 
+// Joystick teleop (issue #212)
+import JoystickTeleop from './components/JoystickTeleop.jsx';
+
 const TAB_GROUPS = [
   {
     label: 'SOCIAL',
@@ -210,7 +213,7 @@ export default function App() {
       </nav>
 
       {/* ── Content ── */}
-      <main className={`flex-1 ${activeTab === 'eventlog' ? 'flex flex-col' : 'overflow-y-auto'} p-4`}>
+      <main className={`flex-1 ${['eventlog', 'control'].includes(activeTab) ? 'flex flex-col' : 'overflow-y-auto'} p-4`}>
         {activeTab === 'status'       && <StatusPanel     subscribe={subscribe} />}
         {activeTab === 'faces'        && <FaceGallery     subscribe={subscribe} callService={callService} />}
         {activeTab === 'conversation' && <ConversationLog subscribe={subscribe} />}
@@ -221,7 +224,16 @@ export default function App() {
         {activeTab === 'battery' && <BatteryPanel subscribe={subscribe} />}
         {activeTab === 'motors'  && <MotorPanel   subscribe={subscribe} />}
         {activeTab === 'map'     && <MapViewer    subscribe={subscribe} />}
-        {activeTab === 'control' && <ControlMode  subscribe={subscribe} />}
+        {activeTab === 'control' && (
+          <div className="flex flex-col h-full gap-4">
+            <div className="flex-1 overflow-y-auto">
+              <ControlMode subscribe={subscribe} />
+            </div>
+            <div className="flex-1 overflow-y-auto">
+              <JoystickTeleop publish={publishFn} />
+            </div>
+          </div>
+        )}
         {activeTab === 'health'  && <SystemHealth subscribe={subscribe} />}
         {activeTab === 'cameras' && <CameraViewer subscribe={subscribe} />}
 

ui/social-bot/src/components/JoystickTeleop.jsx

@@ -0,0 +1,339 @@
+/**
+ * JoystickTeleop.jsx — Virtual joystick for robot teleop
+ *
+ * Features:
+ *   - Virtual thumbstick UI with visual feedback
+ *   - Touch & mouse support
+ *   - Publishes geometry_msgs/Twist to /cmd_vel
+ *   - 10% deadzone for both axes
+ *   - Max linear velocity: 0.5 m/s
+ *   - Max angular velocity: 1.0 rad/s
+ *   - Real-time velocity display
+ */
+
+import { useEffect, useRef, useState } from 'react';
+
+const DEADZONE = 0.1; // 10%
+const MAX_LINEAR_VEL = 0.5; // m/s
+const MAX_ANGULAR_VEL = 1.0; // rad/s
+const RADIUS = 80; // pixels
+const STICK_RADIUS = 25; // pixels
+
+function JoystickTeleop({ publish }) {
+  const canvasRef = useRef(null);
+  const containerRef = useRef(null);
+  const [joystickPos, setJoystickPos] = useState({ x: 0, y: 0 });
+  const [isDragging, setIsDragging] = useState(false);
+  const [linearVel, setLinearVel] = useState(0);
+  const [angularVel, setAngularVel] = useState(0);
+  const velocityTimerRef = useRef(null);
+
+  // Draw joystick on canvas
+  useEffect(() => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+
+    const ctx = canvas.getContext('2d');
+    const rect = canvas.getBoundingClientRect();
+    const centerX = canvas.width / 2;
+    const centerY = canvas.height / 2;
+
+    // Clear canvas
+    ctx.fillStyle = '#1f2937';
+    ctx.fillRect(0, 0, canvas.width, canvas.height);
+
+    // Draw outer circle (background)
+    ctx.strokeStyle = '#374151';
+    ctx.lineWidth = 2;
+    ctx.beginPath();
+    ctx.arc(centerX, centerY, RADIUS, 0, Math.PI * 2);
+    ctx.stroke();
+
+    // Draw crosshair
+    ctx.strokeStyle = '#4b5563';
+    ctx.lineWidth = 1;
+    ctx.beginPath();
+    ctx.moveTo(centerX - 30, centerY);
+    ctx.lineTo(centerX + 30, centerY);
+    ctx.moveTo(centerX, centerY - 30);
+    ctx.lineTo(centerX, centerY + 30);
+    ctx.stroke();
+
+    // Draw deadzone indicator
+    ctx.strokeStyle = '#6b7280';
+    ctx.lineWidth = 1;
+    ctx.setLineDash([2, 2]);
+    ctx.beginPath();
+    ctx.arc(centerX, centerY, RADIUS * DEADZONE, 0, Math.PI * 2);
+    ctx.stroke();
+    ctx.setLineDash([]);
+
+    // Draw stick
+    const stickX = centerX + joystickPos.x * RADIUS;
+    const stickY = centerY + joystickPos.y * RADIUS;
+
+    ctx.fillStyle = isDragging ? '#06b6d4' : '#0891b2';
+    ctx.beginPath();
+    ctx.arc(stickX, stickY, STICK_RADIUS, 0, Math.PI * 2);
+    ctx.fill();
+
+    // Draw direction indicator (arrow)
+    if (Math.abs(joystickPos.x) > 0.01 || Math.abs(joystickPos.y) > 0.01) {
+      const angle = Math.atan2(-joystickPos.y, joystickPos.x);
+      const arrowLen = 40;
+      const arrowX = centerX + Math.cos(angle) * arrowLen;
+      const arrowY = centerY - Math.sin(angle) * arrowLen;
+
+      ctx.strokeStyle = '#06b6d4';
+      ctx.lineWidth = 2;
+      ctx.beginPath();
+      ctx.moveTo(centerX, centerY);
+      ctx.lineTo(arrowX, arrowY);
+      ctx.stroke();
+    }
+  }, [joystickPos, isDragging]);
+
+  // Handle joystick input
+  const handleInput = (clientX, clientY) => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+
+    const rect = canvas.getBoundingClientRect();
+    const centerX = canvas.width / 2;
+    const centerY = canvas.height / 2;
+
+    // Calculate relative position
+    let x = (clientX - rect.left - centerX) / RADIUS;
+    let y = (clientY - rect.top - centerY) / RADIUS;
+
+    // Normalize to unit circle
+    const distance = Math.sqrt(x * x + y * y);
+    if (distance > 1) {
+      x /= distance;
+      y /= distance;
+    }
+
+    // Apply deadzone
+    const deadzoneMagnitude = Math.sqrt(x * x + y * y);
+    let adjustedX = x;
+    let adjustedY = y;
+
+    if (deadzoneMagnitude < DEADZONE) {
+      adjustedX = 0;
+      adjustedY = 0;
+    } else {
+      // Scale values above deadzone
+      const scale = (deadzoneMagnitude - DEADZONE) / (1 - DEADZONE);
+      adjustedX = (x / deadzoneMagnitude) * scale;
+      adjustedY = (y / deadzoneMagnitude) * scale;
+    }
+
+    setJoystickPos({ x: adjustedX, y: adjustedY });
+
+    // Calculate velocities
+    // X-axis (left-right) = angular velocity
+    // Y-axis (up-down) = linear velocity (negative because canvas Y is inverted)
+    const linear = -adjustedY * MAX_LINEAR_VEL;
+    const angular = adjustedX * MAX_ANGULAR_VEL;
+
+    setLinearVel(linear);
+    setAngularVel(angular);
+
+    // Publish Twist message
+    if (publish) {
+      const twist = {
+        linear: { x: linear, y: 0, z: 0 },
+        angular: { x: 0, y: 0, z: angular }
+      };
+      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
+    }
+  };
+
+  const handleMouseDown = (e) => {
+    setIsDragging(true);
+    handleInput(e.clientX, e.clientY);
+  };
+
+  const handleMouseMove = (e) => {
+    if (isDragging) {
+      handleInput(e.clientX, e.clientY);
+    }
+  };
+
+  const handleMouseUp = () => {
+    setIsDragging(false);
+    setJoystickPos({ x: 0, y: 0 });
+    setLinearVel(0);
+    setAngularVel(0);
+
+    if (publish) {
+      const twist = {
+        linear: { x: 0, y: 0, z: 0 },
+        angular: { x: 0, y: 0, z: 0 }
+      };
+      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
+    }
+  };
+
+  // Touch support
+  const handleTouchStart = (e) => {
+    setIsDragging(true);
+    const touch = e.touches[0];
+    handleInput(touch.clientX, touch.clientY);
+  };
+
+  const handleTouchMove = (e) => {
+    if (isDragging) {
+      e.preventDefault();
+      const touch = e.touches[0];
+      handleInput(touch.clientX, touch.clientY);
+    }
+  };
+
+  const handleTouchEnd = () => {
+    setIsDragging(false);
+    setJoystickPos({ x: 0, y: 0 });
+    setLinearVel(0);
+    setAngularVel(0);
+
+    if (publish) {
+      const twist = {
+        linear: { x: 0, y: 0, z: 0 },
+        angular: { x: 0, y: 0, z: 0 }
+      };
+      publish('/cmd_vel', 'geometry_msgs/Twist', twist);
+    }
+  };
+
+  useEffect(() => {
+    const canvas = canvasRef.current;
+    if (!canvas) return;
+
+    canvas.addEventListener('mousedown', handleMouseDown);
+    window.addEventListener('mousemove', handleMouseMove);
+    window.addEventListener('mouseup', handleMouseUp);
+    canvas.addEventListener('touchstart', handleTouchStart);
+    canvas.addEventListener('touchmove', handleTouchMove);
+    canvas.addEventListener('touchend', handleTouchEnd);
+
+    return () => {
+      canvas.removeEventListener('mousedown', handleMouseDown);
+      window.removeEventListener('mousemove', handleMouseMove);
+      window.removeEventListener('mouseup', handleMouseUp);
+      canvas.removeEventListener('touchstart', handleTouchStart);
+      canvas.removeEventListener('touchmove', handleTouchMove);
+      canvas.removeEventListener('touchend', handleTouchEnd);
+    };
+  }, [isDragging]);
+
+  // Cleanup on unmount
+  useEffect(() => {
+    return () => {
+      if (velocityTimerRef.current) {
+        clearTimeout(velocityTimerRef.current);
+      }
+      // Send stop command on unmount
+      if (publish) {
+        const twist = {
+          linear: { x: 0, y: 0, z: 0 },
+          angular: { x: 0, y: 0, z: 0 }
+        };
+        publish('/cmd_vel', 'geometry_msgs/Twist', twist);
+      }
+    };
+  }, [publish]);
+
+  return (
+    <div className="flex flex-col h-full space-y-4">
+      <div className="bg-gray-950 rounded-lg border border-cyan-950 p-4 space-y-3">
+        <div className="text-cyan-700 text-xs font-bold tracking-widest">
+          JOYSTICK TELEOP
+        </div>
+
+        <div className="flex gap-6">
+          {/* Joystick canvas */}
+          <div
+            ref={containerRef}
+            className="relative bg-gray-900 rounded border border-gray-800 p-2"
+          >
+            <canvas
+              ref={canvasRef}
+              width={320}
+              height={320}
+              className="rounded cursor-grab active:cursor-grabbing touch-none"
+              style={{ userSelect: 'none' }}
+            />
+          </div>
+
+          {/* Controls and info */}
+          <div className="flex flex-col justify-center space-y-4 flex-1 min-w-64">
+            {/* Velocity displays */}
+            <div className="space-y-2">
+              <div className="text-sm">
+                <div className="text-gray-500 text-xs mb-1">LINEAR VELOCITY</div>
+                <div className="flex items-end gap-2">
+                  <span className="text-xl font-mono text-cyan-400">
+                    {linearVel.toFixed(3)}
+                  </span>
+                  <span className="text-xs text-gray-600 mb-0.5">m/s</span>
+                </div>
+                <div className="text-xs text-gray-700 mt-1">
+                  Range: -0.500 to +0.500 m/s
+                </div>
+              </div>
+            </div>
+
+            <div className="w-full h-px bg-gray-800" />
+
+            <div className="space-y-2">
+              <div className="text-sm">
+                <div className="text-gray-500 text-xs mb-1">ANGULAR VELOCITY</div>
+                <div className="flex items-end gap-2">
+                  <span className="text-xl font-mono text-yellow-400">
+                    {angularVel.toFixed(3)}
+                  </span>
+                  <span className="text-xs text-gray-600 mb-0.5">rad/s</span>
+                </div>
+                <div className="text-xs text-gray-700 mt-1">
+                  Range: -1.000 to +1.000 rad/s
+                </div>
+              </div>
+            </div>
+
+            <div className="w-full h-px bg-gray-800" />
+
+            {/* Instructions */}
+            <div className="space-y-1 text-xs text-gray-600">
+              <div className="font-bold text-gray-500">CONTROLS:</div>
+              <div>▲ Forward / ▼ Backward</div>
+              <div>◄ Rotate Left / ► Rotate Right</div>
+              <div className="text-gray-700 mt-2">Deadzone: 10%</div>
+            </div>
+
+            {/* Status indicators */}
+            <div className="flex gap-2">
+              <div className={`flex items-center gap-1 px-2 py-1 rounded text-xs ${
+                isDragging
+                  ? 'bg-cyan-950 border border-cyan-700 text-cyan-400'
+                  : 'bg-gray-900 border border-gray-800 text-gray-600'
+              }`}>
+                <div className={`w-2 h-2 rounded-full ${isDragging ? 'bg-cyan-400' : 'bg-gray-700'}`} />
+                {isDragging ? 'ACTIVE' : 'READY'}
+              </div>
+            </div>
+          </div>
+        </div>
+      </div>
+
+      {/* Topic info */}
+      <div className="bg-gray-950 rounded border border-gray-800 p-3 text-xs text-gray-600">
+        <div className="flex justify-between">
+          <span>Publishing to: <code className="text-gray-500">/cmd_vel</code></span>
+          <span className="text-gray-700">geometry_msgs/Twist</span>
+        </div>
+      </div>
+    </div>
+  );
+}
+
+export default JoystickTeleop;