♻️ refactor TestESC to template and cleanup formatting

Author: Apehaenger

robots/include/sabo_robot.hpp

@@ -47,7 +47,15 @@ class SaboRobot : public MowerRobot {
     return charger_.getChargerStatus();
   }
 
-  bool TestESC(VescDriver& motor_driver);
+  template <typename EscDriver>
+  bool TestESC(EscDriver& motor_driver) {
+    if (!motor_driver.IsStarted()) return false;
+    if (motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_DISCONNECTED) {
+      motor_driver.RequestStatus();
+      chThdSleepMilliseconds(100);  // give it a chance to respond
+    }
+    return motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
+  }
   bool TestLeftESC() {
     return TestESC(left_motor_driver_);
   }

robots/src/sabo_robot.cpp

@@ -44,15 +44,3 @@ bool SaboRobot::IsHardwareSupported() {
 
   return false;
 }
-
-bool SaboRobot::TestESC(VescDriver& motor_driver) {
-  if (!motor_driver.IsStarted()) return false;
-
-  if (motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_DISCONNECTED) {
-    motor_driver.RequestStatus();
-    // Give ESC driver some time to respond so that callee don't need to go into retry loop
-    chThdSleepMilliseconds(100);
-  }
-
-  return motor_driver.GetLatestState().status == MotorDriver::ESCState::ESCStatus::ESC_STATUS_OK;
-}