From a9aa86861d0e08b1c2fce92d40f6b0e108bf535a Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 15 Nov 2024 12:16:09 -0500 Subject: [PATCH 1/3] fix(motor-control): add a state to the motor control that maintains the difference between a never homed and a stopped gripper --- .../brushed_motor/brushed_motor_interrupt_handler.hpp | 2 +- include/motor-control/core/types.hpp | 5 +++-- .../tests/test_brushed_motor_interrupt_handler.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 7 deletions(-) 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 436311fad..06831f330 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 @@ -199,7 +199,7 @@ class BrushedMotorInterruptHandler { cancel_and_clear_moves(can::ids::ErrorCode::estop_detected); } else if (hardware.has_cancel_request()) { if (!hardware.get_stay_enabled()) { - hardware.set_motor_state(BrushedMotorState::UNHOMED); + hardware.set_motor_state(BrushedMotorState::STOPPED); } cancel_and_clear_moves(can::ids::ErrorCode::stop_requested, can::ids::ErrorSeverity::warning); diff --git a/include/motor-control/core/types.hpp b/include/motor-control/core/types.hpp index ebc761340..b0e35e00f 100644 --- a/include/motor-control/core/types.hpp +++ b/include/motor-control/core/types.hpp @@ -44,5 +44,6 @@ enum class BrushedMotorState : uint8_t { UNHOMED = 0x0, FORCE_CONTROLLING_HOME = 0x1, FORCE_CONTROLLING = 0x2, - POSITION_CONTROLLING = 0x3 -}; \ No newline at end of file + POSITION_CONTROLLING = 0x3, + STOPPED = 0x4 +}; diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 578754c46..c4f9a1fba 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -526,10 +526,10 @@ SCENARIO("handler recovers from error state") { test_objs.hw.request_cancel(); test_objs.handler.run_interrupt(); THEN( - "motor state should become un-homed only if stay engaged is " - "falsy") { + "motor state should become stopped only if stay engaged is " + "false") { REQUIRE(test_objs.hw.get_motor_state() == - (!stay_engaged ? BrushedMotorState::UNHOMED + (!stay_engaged ? BrushedMotorState::STOPPED : og_motor_state)); } THEN("a stop requested warning is issued") { @@ -560,4 +560,4 @@ SCENARIO("handler recovers from error state") { } } } -} \ No newline at end of file +} From b3856869e97ed380ca7d6fef493d2fb4d76a4614 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 15 Nov 2024 12:19:28 -0500 Subject: [PATCH 2/3] 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, From 6bb855373f5e0a2fc1f29efb5078cb3fa9bced52 Mon Sep 17 00:00:00 2001 From: Ryan howard Date: Fri, 15 Nov 2024 14:46:29 -0500 Subject: [PATCH 3/3] keep the older estop behavior --- .../core/brushed_motor/brushed_motor_interrupt_handler.hpp | 1 - 1 file changed, 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 0d0cbfa0d..e78538826 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 @@ -199,7 +199,6 @@ 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() && hardware.get_motor_state() != BrushedMotorState::UNHOMED) {