use stay_engaged to determine whether or not to stay enabled (#711)

Author: ahiuchingau

include/can/core/messages.hpp

@@ -1005,26 +1005,34 @@ struct GripperGripRequest : BaseMessage<MessageId::gripper_grip_request> {
     uint8_t seq_id;
     brushed_timer_ticks duration;
     uint32_t duty_cycle;
+    int32_t encoder_position_um;
+    uint8_t stay_engaged;
 
     template <bit_utils::ByteIterator Input, typename Limit>
     static auto parse(Input body, Limit limit) -> GripperGripRequest {
         uint8_t group_id = 0;
         uint8_t seq_id = 0;
         brushed_timer_ticks duration = 0;
         uint32_t duty_cycle = 0;
+        int32_t encoder_position_um = 0;
+        uint8_t stay_engaged = 0;
         uint32_t msg_ind = 0;
 
         body = bit_utils::bytes_to_int(body, limit, msg_ind);
         body = bit_utils::bytes_to_int(body, limit, group_id);
         body = bit_utils::bytes_to_int(body, limit, seq_id);
         body = bit_utils::bytes_to_int(body, limit, duration);
         body = bit_utils::bytes_to_int(body, limit, duty_cycle);
+        body = bit_utils::bytes_to_int(body, limit, encoder_position_um);
+        body = bit_utils::bytes_to_int(body, limit, stay_engaged);
 
         return GripperGripRequest{.message_index = msg_ind,
                                   .group_id = group_id,
                                   .seq_id = seq_id,
                                   .duration = duration,
-                                  .duty_cycle = duty_cycle};
+                                  .duty_cycle = duty_cycle,
+                                  .encoder_position_um = encoder_position_um,
+                                  .stay_engaged = stay_engaged};
     }
 
     auto operator==(const GripperGripRequest& other) const -> bool = default;

include/motor-control/core/brushed_motor/brushed_motion_controller.hpp

@@ -48,6 +48,7 @@ class MotionController {
             .duty_cycle = can_msg.duty_cycle,
             .group_id = can_msg.group_id,
             .seq_id = can_msg.seq_id,
+            .stay_engaged = can_msg.stay_engaged,
             .stop_condition = MoveStopCondition::none,
             .usage_key = hardware.get_usage_eeprom_config().get_distance_key()};
         if (!enabled) {

include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp

@@ -251,6 +251,8 @@ class BrushedMotorInterruptHandler {
                         .seconds = uint16_t(
                             hardware.get_stopwatch_pulses(true) / 2600)});
             }
+            hardware.set_stay_enabled(
+                static_cast<bool>(buffered_move.stay_engaged));
             motor_state = ControlState::ACTIVE;
             buffered_move.start_encoder_position =
                 hardware.get_encoder_pulses();
@@ -260,7 +262,6 @@ class BrushedMotorInterruptHandler {
         }
         // clear the old states
         hardware.reset_control();
-        hardware.set_stay_enabled(false);
         timeout_ticks = 0;
         switch (buffered_move.stop_condition) {
             case MoveStopCondition::limit_switch:
@@ -323,16 +324,6 @@ class BrushedMotorInterruptHandler {
             message_index = buffered_move.message_index;
         }
 
-        // if we think we dropped a labware we don't want the controller
-        // to stop the motor in case we only slipped or collided and still
-        // have the labware in the jaws
-        // likewise if the estop is hit and the motor is gripping something
-        if (err_code == can::ids::ErrorCode::labware_dropped ||
-            (motor_state == ControlState::FORCE_CONTROLLING &&
-             err_code == can::ids::ErrorCode::estop_detected)) {
-            hardware.set_stay_enabled(true);
-        }
-
         status_queue_client.send_brushed_move_status_reporter_queue(
             can::messages::ErrorMessage{.message_index = message_index,
                                         .severity = severity,

include/motor-control/core/motor_messages.hpp

@@ -107,6 +107,7 @@ struct BrushedMove {  // NOLINT(cppcoreguidelines-pro-type-member-init)
     uint8_t group_id;
     uint8_t seq_id;
     int32_t encoder_position;
+    uint8_t stay_engaged = 0;
     MoveStopCondition stop_condition = MoveStopCondition::none;
     int32_t start_encoder_position;
     uint16_t usage_key;

motor-control/tests/test_brushed_motor_interrupt_handler.cpp

@@ -37,6 +37,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
                         .duty_cycle = 50,
                         .group_id = 0,
                         .seq_id = 0,
+                        .stay_engaged = 0,
                         .stop_condition = MoveStopCondition::limit_switch};
         test_objs.queue.try_write_isr(msg);
 
@@ -49,6 +50,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
             THEN("The motor hardware proceeds to home") {
                 /* motor shouldn't be gripping */
                 REQUIRE(!test_objs.hw.get_is_gripping());
+                REQUIRE(!test_objs.hw.get_stay_enabled());
                 test_objs.hw.set_encoder_value(1000);
                 test_objs.hw.set_limit_switch(true);
 
@@ -67,6 +69,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
                         REQUIRE(read_ack.ack_id ==
                                 AckMessageId::stopped_by_condition);
                         REQUIRE(test_objs.handler.is_idle);
+                        REQUIRE(!test_objs.hw.get_stay_enabled());
                     }
                 }
             }
@@ -79,6 +82,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
             REQUIRE(test_objs.reporter.messages.size() == 1);
             Ack read_ack = std::get<Ack>(test_objs.reporter.messages.back());
             REQUIRE(read_ack.ack_id == AckMessageId::timeout);
+            REQUIRE(!test_objs.hw.get_stay_enabled());
         }
     }
 
@@ -87,6 +91,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
                                .duty_cycle = 50,
                                .group_id = 0,
                                .seq_id = 0,
+                               .stay_engaged = 1,
                                .stop_condition = MoveStopCondition::none};
         test_objs.queue.try_write_isr(msg);
 
@@ -99,6 +104,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
             THEN("The motor hardware proceeds to grip") {
                 /* motor should be gripping */
                 REQUIRE(test_objs.hw.get_is_gripping());
+                REQUIRE(test_objs.hw.get_stay_enabled());
                 test_objs.hw.set_encoder_value(30000);
 
                 AND_WHEN("The encoder speed timer overflows") {
@@ -125,6 +131,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
                             REQUIRE(read_ack.encoder_position == 30000);
                             REQUIRE(read_ack.ack_id ==
                                     AckMessageId::complete_without_condition);
+                            REQUIRE(test_objs.hw.get_stay_enabled());
                         }
                     }
                 }
@@ -140,6 +147,7 @@ SCENARIO("Brushed motor interrupt handler handle move messages") {
             REQUIRE(test_objs.reporter.messages.size() == 1);
             Ack read_ack = std::get<Ack>(test_objs.reporter.messages.back());
             REQUIRE(read_ack.ack_id == AckMessageId::timeout);
+            REQUIRE(test_objs.hw.get_stay_enabled());
         }
     }
     GIVEN("A message to move") {
@@ -226,13 +234,15 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") {
                         .duty_cycle = 50,
                         .group_id = 0,
                         .seq_id = 0,
+                        .stay_engaged = 1,
                         .stop_condition = MoveStopCondition::limit_switch};
         test_objs.queue.try_write_isr(msg);
         WHEN("Estop is pressed") {
             // Burn through the startup ticks
             for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) {
                 test_objs.handler.run_interrupt();
             }
+            REQUIRE(test_objs.hw.get_stay_enabled());
             test_objs.hw.set_estop_in(true);
             test_objs.handler.run_interrupt();
             THEN("Errors are sent") {
@@ -242,6 +252,7 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") {
                     std::get<can::messages::ErrorMessage>(
                         test_objs.reporter.messages.front());
                 REQUIRE(err.error_code == can::ids::ErrorCode::estop_detected);
+                REQUIRE(test_objs.hw.get_stay_enabled());
             }
         }
     }
@@ -255,6 +266,7 @@ SCENARIO("labware dropped during grip move") {
                                .duty_cycle = 50,
                                .group_id = 0,
                                .seq_id = 0,
+                               .stay_engaged = 1,
                                .stop_condition = MoveStopCondition::none};
         test_objs.queue.try_write_isr(msg);
         WHEN("grip is complete") {
@@ -263,6 +275,7 @@ SCENARIO("labware dropped during grip move") {
             for (uint32_t i = 0; i <= 100; i++) {
                 test_objs.handler.run_interrupt();
             }
+            REQUIRE(test_objs.hw.get_stay_enabled());
             test_objs.handler.set_enc_idle_state(true);
             test_objs.handler.run_interrupt();
             THEN("Move complete message is sent") {
@@ -303,6 +316,7 @@ SCENARIO("collision while homed") {
                         .duty_cycle = 50,
                         .group_id = 0,
                         .seq_id = 0,
+                        .stay_engaged = 0,
                         .stop_condition = MoveStopCondition::limit_switch};
         test_objs.queue.try_write_isr(msg);
         WHEN("home is complete") {