THis should work need to debug why it's triggering a stall in the lim…

…it backoff test or re-write the test
Opentrons · Jan 30, 2025 · e3844e6 · e3844e6
1 parent a3cd34c
commit e3844e6
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 0 deletions.
diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -89,6 +89,26 @@ class MotorInterruptHandler {
                 }
             }
             hardware.unstep();
+        } else {
+            if (!_has_active_move && !has_move_messages()) {
+                printf("no move\n");
+                if (stall_detected()) {
+                    printf("stall\n");
+                    if (!stall_handled) {
+                        printf("stall not handled\n");
+                        if (stall_expected) {
+                            printf("It's ok its expected");
+                        } else {
+                            if (!hardware.position_flags.check_flag(MotorPositionStatus::Flags::encoder_position_ok)) {
+                                printf("all good we dont know where we are\n");
+                            } else {
+                                printf("steps %d encoder %d\n", hardware.get_step_tracker(), hardware.get_encoder_pulses());
+                                handle_stall_without_movement();
+                            }
+                        }
+                    }
+                }
+            }
         }
     }
 
@@ -101,12 +121,22 @@ class MotorInterruptHandler {
         return false;
     }
 
+    auto handle_stall_without_movement() -> void {
+        printf("sending error\n");
+        hardware.position_flags.clear_flag(
+                MotorPositionStatus::Flags::stepper_position_ok);
+        cancel_and_clear_moves(can::ids::ErrorCode::collision_detected);
+        printf("stall handled\n");
+        stall_handled = true;
+    }
+
     auto handle_stall_during_movement() -> void {
         if (!_has_active_move or
             hardware.position_flags.check_flag(
                 MotorPositionStatus::Flags::stepper_position_ok) or
             buffered_move.check_stop_condition(
                 MoveStopCondition::limit_switch_backoff)) {
+            stall_expected = true;
             return;
         }
         if (buffered_move.check_stop_condition(
@@ -128,6 +158,7 @@ class MotorInterruptHandler {
             // update
             finish_current_move(AckMessageId::stopped_by_condition);
             clear_queue_until_empty = true;
+            stall_expected = true;
         } else if (buffered_move.check_stop_condition(
                        MoveStopCondition::ignore_stalls)) {
             if (stall_handled) {
@@ -143,6 +174,7 @@ class MotorInterruptHandler {
                 usage_messages::IncreaseErrorCount{
                     .key = hardware.get_usage_eeprom_config()
                                .get_error_count_key()});
+            stall_expected = true;
         } else {
             cancel_and_clear_moves(can::ids::ErrorCode::collision_detected);
         }
@@ -514,13 +546,15 @@ class MotorInterruptHandler {
         _has_active_move = false;
         tick_count = 0x0;
         stall_handled = false;
+        printf("move finished stall not handled\n");
         build_and_send_ack(ack_msg_id);
         set_buffered_move(MotorMoveMessage{});
         // update the stall check ideal encoder counts based on
         // last known location
         if (!has_move_messages()) {
             stall_checker.reset_itr_counts(hardware.get_step_tracker());
         }
+        printf("move done \n");
     }
 
     void reset() {
@@ -537,6 +571,8 @@ class MotorInterruptHandler {
         hardware.reset_encoder_pulses();
         stall_checker.reset_itr_counts(0);
         stall_handled = false;
+        stall_expected = false;
+        printf("stall not handled reset\n");
     }
 
     [[nodiscard]] static auto overflow(q31_31 current, q31_31 future) -> bool {
@@ -692,6 +728,7 @@ class MotorInterruptHandler {
     MotorMoveMessage buffered_move = MotorMoveMessage{};
     bool clear_queue_until_empty = false;
     bool stall_handled = false;
+    bool stall_expected = false;
     bool in_estop = false;
     std::atomic_bool _has_active_move = false;
 };

diff --git a/motor-control/tests/test_limit_switch_backoff.cpp b/motor-control/tests/test_limit_switch_backoff.cpp
@@ -209,6 +209,7 @@ SCENARIO(
             REQUIRE(test_objs.handler.has_active_move());
 
             AND_WHEN("the limit switch has not been triggered") {
+                printf("running intterupt\n");
                 for (int i = 0; i < (int)msg1.duration; ++i) {
                     test_objs.handler.run_interrupt();
                 }
@@ -221,6 +222,7 @@ SCENARIO(
                 THEN(
                     "the move should be stopped with ack id = stopped without "
                     "condition") {
+                    printf("here's where we check\n");
                     REQUIRE(test_objs.reporter.messages.size() == 1);
                     Ack read_ack =
                         std::get<Ack>(test_objs.reporter.messages.front());

diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp
@@ -137,6 +137,7 @@ SCENARIO("motor handler stall detection") {
                     REQUIRE(ack_msg.message_index == 13);
                     test_objs.reporter.messages.clear();
                     WHEN("the interrupt runs again") {
+                        printf("running interrupt");
                         for (int i = 0; i < (int)msg2.duration; ++i) {
                             test_objs.handler.run_interrupt();
                         }