Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(gripper): add stay engaged param to grip message #711

Merged
merged 1 commit into from
Aug 2, 2023
Merged

feat(gripper): add stay engaged param to grip message #711

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 9 additions & 1 deletion include/can/core/messages.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why are we adding this in the request from host?

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;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why are we adding this here?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's always part of the payload of the generic gripper move messages, but we never defined it because it is always 0 for the gripper grip request

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;
Expand Down
1 change: 1 addition & 0 deletions include/motor-control/core/brushed_motor/brushed_motion_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
13 changes: 2 additions & 11 deletions include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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:
Expand Down Expand Up @@ -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,
Expand Down
1 change: 1 addition & 0 deletions include/motor-control/core/motor_messages.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
14 changes: 14 additions & 0 deletions motor-control/tests/test_brushed_motor_interrupt_handler.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand All @@ -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());
}
}
}
Expand All @@ -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());
}
}

Expand All @@ -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);

Expand All @@ -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") {
Expand All @@ -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());
}
}
}
Expand All @@ -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") {
Expand Down Expand Up @@ -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") {
Expand All @@ -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());
}
}
}
Expand All @@ -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") {
Expand All @@ -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") {
Expand Down Expand Up @@ -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") {
Expand Down
Loading