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

feat(gripper): report jaw encoder pulses when something goes wrong #722

Merged
merged 1 commit into from
Sep 19, 2023
Merged

feat(gripper): report jaw encoder pulses when something goes wrong #722

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
21 changes: 19 additions & 2 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 @@ -138,17 +138,18 @@ class BrushedMotorInterruptHandler {
update_and_start_move();
} else if (!error_handled) {
auto motor_state = hardware.get_motor_state();
auto pulses = hardware.get_encoder_pulses();
// has not reported an error yet
if (motor_state == BrushedMotorState::POSITION_CONTROLLING) {
int32_t move_delta =
hardware.get_encoder_pulses() - hold_encoder_position;
int32_t move_delta = pulses - hold_encoder_position;
controlled_move_to(move_delta);
// we use a value higher than the acceptable position here to
// allow the pid loop the opportunity to maintain small
// movements that occur from motion and vibration
if (move_delta > error_conf.unwanted_movement_threshold) {
cancel_and_clear_moves(
can::ids::ErrorCode::collision_detected);
report_position(pulses);
error_handled = true;
}
} else if (motor_state != BrushedMotorState::UNHOMED) {
Expand All @@ -162,6 +163,7 @@ class BrushedMotorInterruptHandler {
? can::ids::ErrorCode::labware_dropped
: can::ids::ErrorCode::collision_detected;
cancel_and_clear_moves(err);
report_position(pulses);
error_handled = true;
}
}
Expand Down Expand Up @@ -301,6 +303,21 @@ class BrushedMotorInterruptHandler {
}
}

void report_position(int32_t pulses) {
// this message is used only to report encoder position on the can bus
// when the move fails, and will not be used nor handled by the host
uint32_t message_index = 0;
if (_has_active_move) {
message_index = buffered_move.message_index;
}
status_queue_client.send_brushed_move_status_reporter_queue(
motor_messages::UpdatePositionResponse{
.message_index = message_index,
.stepper_position_counts = 0,
.encoder_pulses = pulses,
.position_flags = 0});
}

void homing_stopped() {
// since the buffered move start position isn't reliable here,
// and then end position will always be 0, we set the buffered move
Expand Down
21 changes: 18 additions & 3 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 @@ -309,13 +309,18 @@ SCENARIO("labware dropped during grip move") {
test_objs.handler.run_interrupt();
}
// An error and increase error count is sent
REQUIRE(test_objs.reporter.messages.size() == 2);
REQUIRE(test_objs.reporter.messages.size() == 3);
can::messages::ErrorMessage err =
std::get<can::messages::ErrorMessage>(
test_objs.reporter.messages.front());
REQUIRE(err.error_code == can::ids::ErrorCode::labware_dropped);
REQUIRE(test_objs.handler.error_handled);
REQUIRE(test_objs.hw.get_stay_enabled());
// position reported
motor_messages::UpdatePositionResponse pos_response =
std::get<motor_messages::UpdatePositionResponse>(
test_objs.reporter.messages.back());
REQUIRE(pos_response.encoder_pulses == 40000);
}
}
}
Expand Down Expand Up @@ -354,14 +359,19 @@ SCENARIO("collision while homed") {
test_objs.handler.run_interrupt();
}
// An error and increase error count is sent
REQUIRE(test_objs.reporter.messages.size() == 2);
REQUIRE(test_objs.reporter.messages.size() == 3);
can::messages::ErrorMessage err =
std::get<can::messages::ErrorMessage>(
test_objs.reporter.messages.front());
REQUIRE(err.error_code ==
can::ids::ErrorCode::collision_detected);
REQUIRE(test_objs.handler.error_handled);
REQUIRE(!test_objs.hw.get_stay_enabled());
// position reported
motor_messages::UpdatePositionResponse pos_response =
std::get<motor_messages::UpdatePositionResponse>(
test_objs.reporter.messages.back());
REQUIRE(pos_response.encoder_pulses == 40000);
}
}
}
Expand Down Expand Up @@ -432,14 +442,19 @@ SCENARIO("A collision during position controlled move") {
test_objs.handler.run_interrupt();
}
// An error and increase error count is sent
REQUIRE(test_objs.reporter.messages.size() == 2);
REQUIRE(test_objs.reporter.messages.size() == 3);
can::messages::ErrorMessage err =
std::get<can::messages::ErrorMessage>(
test_objs.reporter.messages.front());
REQUIRE(err.error_code ==
can::ids::ErrorCode::collision_detected);
REQUIRE(test_objs.handler.error_handled);
REQUIRE(!test_objs.hw.get_stay_enabled());
// position reported
motor_messages::UpdatePositionResponse pos_response =
std::get<motor_messages::UpdatePositionResponse>(
test_objs.reporter.messages.back());
REQUIRE(pos_response.encoder_pulses == 200000);
}
}
}
Expand Down
Loading