From c6d02bbe43eef518351c3fa4743626e7e5618271 Mon Sep 17 00:00:00 2001
From: Matt Spencer <matthew@thespencers.me.uk>
Date: Wed, 27 May 2026 23:10:20 +0000
Subject: [PATCH] Ensure ros controller doesn't timeout on cmd_vel

The ros controller will timeout if it doesn't receive a cmd_vel message
in a given period, stopping the wheels.
webui sends messages at 100ms intervals, so the update rate needs to be 10Hz
This may need to be reviewed when using other input systems.
---
 ansible/setup_robot.yml                       | 15 ++++++++++++
 .../src/raspbot_hardware_interface.cpp        | 23 ++++++++++++++++++-
 2 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/ansible/setup_robot.yml b/ansible/setup_robot.yml
index 0f7f0f8..0a5f9f3 100644
--- a/ansible/setup_robot.yml
+++ b/ansible/setup_robot.yml
@@ -45,5 +45,20 @@
     #     state: present
     #     update_cache: true
 
+    # I2C Fast Mode (400 kHz) reduces SSD1306 OLED frame transfers from ~95 ms
+    # to ~24 ms, shrinking the window in which the kernel I2C adapter lock blocks
+    # motor writes on the shared /dev/i2c-1 bus.
+    - name: Set I2C bus speed to 400 kHz
+      lineinfile:
+        path: /boot/firmware/config.txt
+        regexp: '^dtparam=i2c_arm_baudrate='
+        line: 'dtparam=i2c_arm_baudrate=400000'
+      notify: Reboot required
+
+  handlers:
+    - name: Reboot required
+      debug:
+        msg: "config.txt updated — reboot the Pi to apply I2C speed change"
+
   roles:
     - geerlingguy.docker
diff --git a/raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp b/raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp
index d43678f..5dbce2e 100644
--- a/raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp
+++ b/raspbot_v2/src/raspbot_v2_hardware_interface/src/raspbot_hardware_interface.cpp
@@ -7,6 +7,7 @@
 
 #include <algorithm>
 #include <cerrno>
+#include <chrono>
 #include <cmath>
 #include <cstring>
 #include <string>
@@ -112,6 +113,8 @@ hardware_interface::return_type RaspbotHardwareInterface::read(
 hardware_interface::return_type RaspbotHardwareInterface::write(
   const rclcpp::Time &, const rclcpp::Duration &)
 {
+  const std::array<double, 4> prev_commands = wheel_commands_;
+
   wheel_commands_[0] = get_command<double>("joint_wheel_front_left/velocity");
   wheel_commands_[1] = get_command<double>("joint_wheel_front_right/velocity");
   wheel_commands_[2] = get_command<double>("joint_wheel_rear_left/velocity");
@@ -122,6 +125,14 @@ hardware_interface::return_type RaspbotHardwareInterface::write(
     if (std::isnan(v)) {v = 0.0;}
   }
 
+  static constexpr const char * kNames[] = {"FL", "FR", "RL", "RR"};
+  for (size_t i = 0; i < 4; ++i) {
+    if (prev_commands[i] != 0.0 && wheel_commands_[i] == 0.0) {
+      RCLCPP_WARN(get_logger(), "Wheel %s commanded to zero (was %.3f rad/s)",
+        kNames[i], prev_commands[i]);
+    }
+  }
+
   ctrl_muto(MOTOR_FL, to_motor_speed(wheel_commands_[0]));
   ctrl_muto(MOTOR_FR, to_motor_speed(wheel_commands_[1]));
   ctrl_muto(MOTOR_RL, to_motor_speed(wheel_commands_[2]));
@@ -141,7 +152,17 @@ bool RaspbotHardwareInterface::ctrl_muto(int motor_id, int speed)
   const uint8_t abs_speed = static_cast<uint8_t>(std::abs(clamped));
   // Register 0x01: [motor_id, direction, speed] — mirrors Raspbot_Lib Ctrl_Muto
   uint8_t buf[] = {0x01, static_cast<uint8_t>(motor_id), dir, abs_speed};
-  return ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
+
+  const auto t0 = std::chrono::steady_clock::now();
+  const bool ok = ::write(i2c_fd_, buf, sizeof(buf)) == static_cast<ssize_t>(sizeof(buf));
+  const auto dt_us = std::chrono::duration_cast<std::chrono::microseconds>(
+    std::chrono::steady_clock::now() - t0).count();
+
+  if (dt_us > 1000) {
+    RCLCPP_WARN(get_logger(), "ctrl_muto motor %d: %ld µs", motor_id, dt_us);
+  }
+
+  return ok;
 }
 
 int RaspbotHardwareInterface::to_motor_speed(double rad_s) const