From b3856869e97ed380ca7d6fef493d2fb4d76a4614 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 15 Nov 2024 12:19:28 -0500 Subject: [PATCH] revert to the unhomed state during estop or collisions --- .../core/brushed_motor/brushed_motor_interrupt_handler.hpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp index 06831f330..0d0cbfa0d 100644 --- a/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp +++ b/include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp @@ -151,6 +151,7 @@ class BrushedMotorInterruptHandler { can::ids::ErrorCode::collision_detected); report_position(pulses); error_handled = true; + hardware.set_motor_state(BrushedMotorState::UNHOMED); } } else if (motor_state != BrushedMotorState::UNHOMED) { auto pulses = hardware.get_encoder_pulses(); @@ -166,6 +167,7 @@ class BrushedMotorInterruptHandler { motor_state == BrushedMotorState::FORCE_CONTROLLING ? can::ids::ErrorCode::labware_dropped : can::ids::ErrorCode::collision_detected; + hardware.set_motor_state(BrushedMotorState::UNHOMED); cancel_and_clear_moves(err); report_position(pulses); error_handled = true; @@ -197,8 +199,10 @@ class BrushedMotorInterruptHandler { } else if (estop_triggered()) { in_estop = true; cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); + hardware.set_motor_state(BrushedMotorState::UNHOMED); } else if (hardware.has_cancel_request()) { - if (!hardware.get_stay_enabled()) { + if (!hardware.get_stay_enabled() && + hardware.get_motor_state() != BrushedMotorState::UNHOMED) { hardware.set_motor_state(BrushedMotorState::STOPPED); } cancel_and_clear_moves(can::ids::ErrorCode::stop_requested,