fix(gripper): add stopped state (#816)

* fix(motor-control): add a state to the motor control that maintains the difference between a never homed and a stopped gripper

* revert to the unhomed state during estop or collisions

* keep the older estop behavior

Author: ryanthecoder

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;
@@ -198,8 +200,9 @@ class BrushedMotorInterruptHandler {
             in_estop = true;
             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);
+            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,
                                    can::ids::ErrorSeverity::warning);

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
-};
+    POSITION_CONTROLLING = 0x3,
+    STOPPED = 0x4
+};

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") {
             }
         }
     }
-}
+}