From 2ffc6ce69db6509336964cdf67d7fa744ac46e07 Mon Sep 17 00:00:00 2001 From: Ryan Howard Date: Thu, 24 Oct 2024 15:17:50 -0400 Subject: [PATCH] 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 --- .../core/stepper_motor/motion_controller.hpp | 1 + .../stepper_motor/motor_interrupt_handler.hpp | 15 +++++++++++++-- .../tests/test_motor_stall_handling.cpp | 18 ++++++++++++------ 3 files changed, 26 insertions(+), 8 deletions(-) diff --git a/include/motor-control/core/stepper_motor/motion_controller.hpp b/include/motor-control/core/stepper_motor/motion_controller.hpp index 6f3267fcc..f81d21c0b 100644 --- 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 index fab6d550c..89d3ea43e 100644 --- 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 index f75b2db23..1dbeddf6d 100644 --- 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(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( + 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") { } } } -} \ No newline at end of file +}