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

@@ -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

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