From 3efa6664c586b0d07daf99ab5c05e41ceae98f98 Mon Sep 17 00:00:00 2001 From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 1 Aug 2023 17:24:02 -0400 Subject: [PATCH] use stay_engaged to determine whether or not to stay enabled --- include/can/core/messages.hpp | 10 +++++++++- .../brushed_motor/brushed_motion_controller.hpp | 1 + .../brushed_motor_interrupt_handler.hpp | 13 ++----------- include/motor-control/core/motor_messages.hpp | 1 + .../tests/test_brushed_motor_interrupt_handler.cpp | 14 ++++++++++++++ 5 files changed, 27 insertions(+), 12 deletions(-) diff --git a/include/can/core/messages.hpp b/include/can/core/messages.hpp index 9a0c2f0dc..d73bc66e7 100644 --- a/include/can/core/messages.hpp +++ b/include/can/core/messages.hpp @@ -1005,6 +1005,8 @@ struct GripperGripRequest : BaseMessage { uint8_t seq_id; brushed_timer_ticks duration; uint32_t duty_cycle; + int32_t encoder_position_um; + uint8_t stay_engaged; template static auto parse(Input body, Limit limit) -> GripperGripRequest { @@ -1012,6 +1014,8 @@ struct GripperGripRequest : BaseMessage { 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); @@ -1019,12 +1023,16 @@ struct GripperGripRequest : BaseMessage { 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; diff --git a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp b/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp index 18fbc315d..38a1ff55d 100644 --- a/include/motor-control/core/brushed_motor/brushed_motion_controller.hpp +++ b/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) { 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 a4c8b4ff8..3481827a7 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 @@ -251,6 +251,8 @@ class BrushedMotorInterruptHandler { .seconds = uint16_t( hardware.get_stopwatch_pulses(true) / 2600)}); } + hardware.set_stay_enabled( + static_cast(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, diff --git a/include/motor-control/core/motor_messages.hpp b/include/motor-control/core/motor_messages.hpp index a6acf1c2d..3813702bc 100644 --- a/include/motor-control/core/motor_messages.hpp +++ b/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; diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 69206e224..47e5fe190 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/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(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(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,6 +234,7 @@ 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") { @@ -233,6 +242,7 @@ SCENARIO("estop pressed during Brushed motor interrupt handler") { 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( 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") {