From df421a8b79f0a098de8e92d84a15a3f4fc7c8b29 Mon Sep 17 00:00:00 2001
From: Michael Wimble <mike@wimblerobotics.com>
Date: Tue, 18 Mar 2025 18:57:53 -0700
Subject: [PATCH] Snapshot

---
 include/roboclaw.h | 43 ++++++++++++++++++++++++++++++++++++++++---
 src/roboclaw.cpp   | 39 ++++-----------------------------------
 2 files changed, 44 insertions(+), 38 deletions(-)

diff --git a/include/roboclaw.h b/include/roboclaw.h
index 250ca46..d8ba4d6 100755
--- a/include/roboclaw.h
+++ b/include/roboclaw.h
@@ -429,7 +429,8 @@ private:
     }
 
     void showLog() {
-      RCUTILS_LOG_INFO("%s, READ: %s", write_log_, read_log_);
+      RCUTILS_LOG_INFO("[RoboClaw::DebugLog] %s, READ: %s", write_log_,
+                       read_log_);
       read_log_[0] = '\0';
       next_read_log_index_ = 0;
       write_log_[0] = '\0';
@@ -497,8 +498,6 @@ private:
             responseCrc |= datum;
             if (responseCrc == crc) {
               version_ = version.str();
-              // roboclaw_.debug_log_.appendToReadLog("RR%d", 0);
-              // roboclaw_.debug_log_.appendToWriteLog("WW%d", 0);
               roboclaw_.debug_log_.showLog();
               return;
             } else {
@@ -554,6 +553,44 @@ private:
     long value_;
   };
 
+  class CmdReadStatus : public Cmd {
+  public:
+    CmdReadStatus(RoboClaw &roboclaw, unsigned short &status)
+        : Cmd(roboclaw, "ReadStatus", kNone), status_(status) {}
+    void send() override {
+      try {
+        uint16_t crc = 0;
+        roboclaw_.updateCrc(crc, roboclaw_.portAddress_);
+        roboclaw_.updateCrc(crc, kGETERROR);
+        roboclaw_.appendToWriteLog("ReadStatus: WROTE: ");
+        roboclaw_.writeN2(false, 2, roboclaw_.portAddress_, kGETERROR);
+        unsigned short result = (unsigned short)roboclaw_.getULongCont(crc);
+        uint16_t responseCrc = 0;
+        uint16_t datum = roboclaw_.readByteWithTimeout2();
+        responseCrc = datum << 8;
+        datum = roboclaw_.readByteWithTimeout2();
+        responseCrc |= datum;
+        if (responseCrc == crc) {
+          roboclaw_.appendToReadLog(", RESULT: %04X", result);
+          roboclaw_.debug_log_.showLog();
+          status_ = result;
+          return;
+        } else {
+          RCUTILS_LOG_ERROR(
+              "[RoboClaw::cache_getErrorStatus] invalid CRC expected: 0x%02X, "
+              "got: "
+              "0x%02X",
+              crc, responseCrc);
+        }
+      } catch (...) {
+        RCUTILS_LOG_ERROR(
+            "[RoboClaw::cache_getErrorStatus] Uncaught exception !!!");
+      }
+    };
+
+    unsigned short &status_;
+  };
+
   friend class Cmd; // Make Cmd a friend class of RoboClaw
 
 protected:
diff --git a/src/roboclaw.cpp b/src/roboclaw.cpp
index 56368f1..8bc26c3 100755
--- a/src/roboclaw.cpp
+++ b/src/roboclaw.cpp
@@ -383,41 +383,10 @@ uint16_t RoboClaw::getErrorStatus() {
 }
 
 uint16_t RoboClaw::cache_getErrorStatus() {
-  for (int retry = 0; retry < maxCommandRetries_; retry++) {
-    try {
-      uint16_t crc = 0;
-      updateCrc(crc, portAddress_);
-      updateCrc(crc, kGETERROR);
-
-      writeN(false, 2, portAddress_, kGETERROR);
-      unsigned short result = (unsigned short)getULongCont(crc);
-      uint16_t responseCrc = 0;
-      uint16_t datum = readByteWithTimeout();
-      responseCrc = datum << 8;
-      datum = readByteWithTimeout();
-      responseCrc |= datum;
-      if (responseCrc == crc) {
-        return result;
-      } else {
-        RCUTILS_LOG_ERROR(
-            "[RoboClaw::cache_getErrorStatus] invalid CRC expected: 0x%02X, "
-            "got: "
-            "0x%02X",
-            crc, responseCrc);
-      }
-    } catch (TRoboClawException *e) {
-      RCUTILS_LOG_ERROR(
-          "[RoboClaw::cache_getErrorStatus] Exception: %s, retry number: %d",
-          e->what(), retry);
-    } catch (...) {
-      RCUTILS_LOG_ERROR(
-          "[RoboClaw::cache_getErrorStatus] Uncaught exception !!!");
-    }
-  }
-
-  RCUTILS_LOG_ERROR("[RoboClaw::cache_getErrorStatus] RETRY COUNT EXCEEDED");
-  throw new TRoboClawException(
-      "[RoboClaw::cache_getErrorStatus] RETRY COUNT EXCEEDED");
+  unsigned short result;
+  CmdReadStatus cmd(*this, result);
+  cmd.execute();
+  return result;
 }
 
 std::string RoboClaw::getErrorString() {