fix(motor-control): catch falling 96 channel (#810)

* call disable motor on a stop request so that we engage the brake if needed * catch a stall during a home if the axis starts moving in the wrong direction * fixup the tests for the new behavior * lint
Opentrons · Oct 24, 2024 · 2ffc6ce · 2ffc6ce
1 parent e5e1817
commit 2ffc6ce
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 8 deletions.
diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp
@@ -195,6 +195,7 @@ class MotionController {
         if (hardware.is_timer_interrupt_running()) {
             hardware.set_cancel_request(error_severity, error_code);
         }
+        disable_motor();
     }
 
     void clear_cancel_request() { hardware.clear_cancel_request(); }

diff --git a/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp b/include/motor-control/core/stepper_motor/motor_interrupt_handler.hpp
@@ -107,12 +107,23 @@ class MotorInterruptHandler {
         if (!_has_active_move or
             hardware.position_flags.check_flag(
                 MotorPositionStatus::Flags::stepper_position_ok) or
-            buffered_move.check_stop_condition(
-                MoveStopCondition::limit_switch) or
             buffered_move.check_stop_condition(
                 MoveStopCondition::limit_switch_backoff)) {
             return;
         }
+        if (buffered_move.check_stop_condition(
+                MoveStopCondition::limit_switch)) {
+            // Since the encoders are always setup that negative is towards the
+            // limit switch if the encoder has increased past the start position
+            // we know that we've stalled which means on the z axis that
+            // something is falling so we want to trigger a collision so we can
+            // catch it before it hits something
+            if ((buffered_move.start_encoder_position -
+                 hardware.get_encoder_pulses()) > 10) {
+                return;
+            }
+            cancel_and_clear_moves(can::ids::ErrorCode::collision_detected);
+        }
 
         if (buffered_move.check_stop_condition(MoveStopCondition::stall)) {
             // if expected, finish move and clear queue to prepare for position

diff --git a/motor-control/tests/test_motor_stall_handling.cpp b/motor-control/tests/test_motor_stall_handling.cpp
@@ -308,11 +308,17 @@ SCENARIO("motor handler stall detection") {
                 REQUIRE(!test_objs.hw.position_flags.check_flag(
                     Flags::stepper_position_ok));
                 THEN("move completed and no error was raised") {
-                    REQUIRE(test_objs.reporter.messages.size() == 1);
-                    Ack ack_msg =
-                        std::get<Ack>(test_objs.reporter.messages.front());
-                    REQUIRE(ack_msg.ack_id ==
-                            AckMessageId::complete_without_condition);
+                    REQUIRE(test_objs.reporter.messages.size() == 6);
+                    for (auto& element : test_objs.reporter.messages) {
+                        printf("%ld\n", element.index());
+                    }
+                    can::messages::ErrorMessage err =
+                        std::get<can::messages::ErrorMessage>(
+                            test_objs.reporter.messages.front());
+                    REQUIRE(err.error_code ==
+                            can::ids::ErrorCode::collision_detected);
+                    REQUIRE(err.severity ==
+                            can::ids::ErrorSeverity::unrecoverable);
                 }
             }
         }
@@ -349,4 +355,4 @@ SCENARIO("motor handler stall detection") {
             }
         }
     }
-}
+}