From 7d270e10c07c2060f91467b663e10502bcc84fad Mon Sep 17 00:00:00 2001
From: pmoegenburg <pbm53@cornell.edu>
Date: Tue, 14 Nov 2023 18:08:44 -0500
Subject: [PATCH] debug changes

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

diff --git a/include/motor-control/core/tasks/motion_controller_task.hpp b/include/motor-control/core/tasks/motion_controller_task.hpp
index 4d20b8d9e..90812c95f 100644
--- a/include/motor-control/core/tasks/motion_controller_task.hpp
+++ b/include/motor-control/core/tasks/motion_controller_task.hpp
@@ -228,6 +228,8 @@ class MotionControllerMessageHandler {
                     .severity = can::ids::ErrorSeverity::unrecoverable,
                     .error_code =
                         can::ids::ErrorCode::motor_driver_error_detected}); // delete
+        // TickTypet currentTick = xTaskGetTickCount();
+        // while(xTaskGetTickCount() – currentTick < pdMSTO_TICKS(1000))
         vTaskDelay(pdMS_TO_TICKS(200)); // Need to act immediately?! Just decrease this?
         debounce_count++;
         if (debounce_count > 10) { // send msg immediately, reset flags after delay?!
@@ -240,9 +242,10 @@ class MotionControllerMessageHandler {
             }
             diag0_debounced = false;
             debounce_count = 0;
+        } else {
+            // Run this by Seth
+            motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
         }
-        // Pull queue in via constructor?! Run by Seth
-        motion_client.send_motion_controller_queue(can::messages::DebounceMotorDriverError{.message_index = m.message_index});
     }
 
     MotorControllerType& controller;