From 110b08a3c1b29ecd3551f15913af3a93b3cbbcc1 Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Mon, 13 Nov 2023 17:35:31 -0500
Subject: [PATCH] debug fix

---
 .../core/tasks/motion_controller_task.hpp            | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 0567f5585..807894553 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -164,16 +164,18 @@ class MotionControllerMessageHandler {
 
     void handle(const can::messages::RouteMotorDriverInterrupt& m) {
         static_cast<void>(m);
-        diag0_debounce_count++;
-        if (diag0_debounce_count > 500) {
-            diag0_debounce_count = 0;
-            if (controller.read_tmc_diag0()) {  // debounce needed? But need to act immediately?!
+        if (diag0_debounced) {
+            diag0_debounced = false;
+            if (controller.read_tmc_diag0()) {  // debounce needed?
                 handle_message(
                     can::messages::MotorDriverErrorEncountered{.message_index = 0});
             } else {
                 handle_message(can::messages::ResetMotorDriverErrorHandling{
                     .message_index = 0});
             }
+        } else {
+            vTaskDelay(3000); // Is this ok to use? Need to act immediately?! Just decrease this?
+            diag0_debounced = true;
         }
     }
 
@@ -211,7 +213,7 @@ class MotionControllerMessageHandler {
     CanClient& can_client;
     UsageClient& usage_client;
     DriverClient& driver_client;
-    std::atomic<uint16_t> diag0_debounce_count = 0;
+    std::atomic<bool> diag0_debounced = false;
 };
 
 /**