Skip to content

Commit

Permalink
feat(gripper): allow the encoder value to debounce if deviated from e…
Browse files Browse the repository at this point in the history 
…xpected val (#725)

There's potential noise on the gripper jaw encoder line that causes the gripper to falsely report a move has failed. We're now confirming the encoder value has deviated from the expected val twice consecutively when executing an idle move before reporting it has failed.
  • Loading branch information
@ahiuchingau
ahiuchingau authored Sep 19, 2023
1 parent 50b3ee6 commit fb65580
Show file tree
Hide file tree
Showing 4 changed files with 157 additions and 21 deletions.
68 changes: 68 additions & 0 deletions common/tests/test_debounce.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,3 +32,71 @@ SCENARIO("debouncing gpio pins works") {
}
}
}

SCENARIO("debouncing with holdoff cnt works") {
GIVEN("a state value and bounce value") {
debouncer::Debouncer subject{.holdoff_cnt = 2};
WHEN("switching from unset to set") {
subject.debounce_update(true);
CHECK(subject.cnt == 0);
subject.debounce_update(true);
CHECK(subject.cnt == 1);
THEN("state stays the same on the first two ticks") {
REQUIRE(subject.debounce_state() == false);
}
subject.debounce_update(true);
THEN("state updates on the third tick") {
REQUIRE(subject.debounce_state() == true);
THEN("cnt gets reset") { CHECK(subject.cnt == 0); }
}
WHEN("reset is called") {
subject.reset();
THEN("values are reset") {
REQUIRE(subject.cnt == 0);
REQUIRE(subject.state_bounce == false);
REQUIRE(subject.state == false);
// holdoff cnt remains the same
REQUIRE(subject.holdoff_cnt == 2);
}
}
}
WHEN("switching from set to unset") {
// make sure the state is true
subject.debounce_update(true);
subject.debounce_update(true);
subject.debounce_update(true);

subject.debounce_update(false);
subject.debounce_update(false);
THEN("state stays the same on the first two ticks") {
REQUIRE(subject.debounce_state() == true);
}
subject.debounce_update(false);
THEN("state updates on the third tick") {
REQUIRE(subject.debounce_state() == false);
}
}
WHEN("switching from set to unset before holdoff cnt is reached") {
// make sure the state is true
subject.debounce_update(true);
subject.debounce_update(true);
CHECK(subject.cnt == 1);
subject.debounce_update(false);
THEN("cnt gets reset back to 0") {
CHECK(subject.cnt == 0);
REQUIRE(subject.debounce_state() == false);
}
THEN("it requires another 3 updates to actually update the state") {
subject.debounce_update(true);
CHECK(subject.cnt == 0);
REQUIRE(subject.debounce_state() == false);
subject.debounce_update(true);
CHECK(subject.cnt == 1);
REQUIRE(subject.debounce_state() == false);
subject.debounce_update(true);
CHECK(subject.cnt == 0);
REQUIRE(subject.debounce_state() == true);
}
}
}
}
20 changes: 19 additions & 1 deletion include/common/core/debounce.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,18 +4,36 @@
namespace debouncer {

struct Debouncer {
int holdoff_cnt = 1;
std::atomic_bool state = false;
std::atomic_bool state_bounce = false;
int cnt = 0;

auto debounce_update(bool new_state) -> void {
// only set the state if the bounce matches the current gpio_is_set
// on the first state change it won't match but on the second tick it
// will and we can set it to the new state.

if (new_state == state_bounce) {
state = new_state;
cnt++;
if (cnt == holdoff_cnt) {
// update state only when cnt reaches the holdoff count
state = new_state;
cnt = 0;
}
} else {
// reset count every time the mismatched
cnt = 0;
}
// state bounce is updated each time
state_bounce = new_state;
}
[[nodiscard]] auto debounce_state() const -> bool { return state.load(); }
auto reset() -> void {
state = false;
state_bounce = false;
cnt = 0;
}
};

} // namespace debouncer
47 changes: 29 additions & 18 deletions include/motor-control/core/brushed_motor/brushed_motor_interrupt_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

#include "can/core/ids.hpp"
#include "can/core/messages.hpp"
#include "common/core/debounce.hpp"
#include "common/core/logging.h"
#include "common/core/message_queue.hpp"
#include "motor-control/core/brushed_motor/driver_interface.hpp"
Expand Down Expand Up @@ -146,17 +147,24 @@ class BrushedMotorInterruptHandler {
// 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) {
enc_errored.debounce_update(
move_delta > error_conf.unwanted_movement_threshold);
if (enc_errored.debounce_state()) {
// enc value errored for 3 consecutive ticks
cancel_and_clear_moves(
can::ids::ErrorCode::collision_detected);
report_position(pulses);
error_handled = true;
}
} else if (motor_state != BrushedMotorState::UNHOMED) {
auto pulses = hardware.get_encoder_pulses();
if (!is_idle && pulses >= 0 &&
enc_errored.debounce_update(
!is_idle && pulses >= 0 &&
std::abs(pulses - hold_encoder_position) >
error_conf.unwanted_movement_threshold) {
error_conf.unwanted_movement_threshold);

if (enc_errored.debounce_state()) {
// enc value errored for 3 consecutive ticks
// we have likely dropped a labware or had a collision
auto err =
motor_state == BrushedMotorState::FORCE_CONTROLLING
Expand Down Expand Up @@ -241,29 +249,31 @@ class BrushedMotorInterruptHandler {

void update_and_start_move() {
_has_active_move = queue.try_read_isr(&buffered_move);
if (_has_active_move) {
auto motor_state = hardware.get_motor_state();
if (motor_state == BrushedMotorState::FORCE_CONTROLLING ||
motor_state == BrushedMotorState::FORCE_CONTROLLING_HOME) {
// if we've been applying force before the new move is called
// add that force application time to the usage storage
status_queue_client.send_brushed_move_status_reporter_queue(
usage_messages::IncreaseForceTimeUsage{
.key = hardware.get_usage_eeprom_config()
.get_force_application_time_key(),
.seconds = uint16_t(
hardware.get_stopwatch_pulses(true) / 2600)});
}
buffered_move.start_encoder_position =
hardware.get_encoder_pulses();
if (!_has_active_move) {
return;
}
auto motor_state = hardware.get_motor_state();
if (motor_state == BrushedMotorState::FORCE_CONTROLLING ||
motor_state == BrushedMotorState::FORCE_CONTROLLING_HOME) {
// if we've been applying force before the new move is called
// add that force application time to the usage storage
status_queue_client.send_brushed_move_status_reporter_queue(
usage_messages::IncreaseForceTimeUsage{
.key = hardware.get_usage_eeprom_config()
.get_force_application_time_key(),
.seconds =
uint16_t(hardware.get_stopwatch_pulses(true) / 2600)});
}
buffered_move.start_encoder_position = hardware.get_encoder_pulses();

if (buffered_move.duty_cycle != 0U) {
driver_hardware.update_pwm_settings(buffered_move.duty_cycle);
}
// clear the old states
hardware.reset_control();
timeout_ticks = 0;
error_handled = false;
enc_errored.reset();
hardware.set_stay_enabled(
static_cast<bool>(buffered_move.stay_engaged));
switch (buffered_move.stop_condition) {
Expand Down Expand Up @@ -424,5 +434,6 @@ class BrushedMotorInterruptHandler {
uint32_t current_control_pwm = 0;
bool clear_queue_until_empty = false;
std::atomic_bool _has_active_move = false;
debouncer::Debouncer enc_errored = debouncer::Debouncer{};
};
} // namespace brushed_motor_handler
43 changes: 41 additions & 2 deletions motor-control/tests/test_brushed_motor_interrupt_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -435,10 +435,11 @@ SCENARIO("A collision during position controlled move") {
}
test_objs.reporter.messages.clear();
THEN("Movement starts again") {
REQUIRE(!test_objs.handler.has_active_move());
test_objs.hw.set_encoder_value(200000);
test_objs.handler.set_enc_idle_state(false);
// Burn through the holdoff ticks
for (uint32_t i = 0; i <= HOLDOFF_TICKS; i++) {
// Burn through the 2 x holdoff ticks
for (uint32_t i = 0; i <= HOLDOFF_TICKS * 2; i++) {
test_objs.handler.run_interrupt();
}
// An error and increase error count is sent
Expand All @@ -460,6 +461,44 @@ SCENARIO("A collision during position controlled move") {
}
}

SCENARIO("handler encounter error during idle move") {
BrushedMotorContainer test_objs{};
auto og_motor_state = GENERATE(BrushedMotorState::FORCE_CONTROLLING_HOME,
BrushedMotorState::FORCE_CONTROLLING,
BrushedMotorState::POSITION_CONTROLLING);

GIVEN("the encoder value begins to error during an idle move") {
test_objs.hw.set_motor_state(og_motor_state);
test_objs.hw.set_encoder_value(200000);
test_objs.handler.set_enc_idle_state(false);

WHEN("the first idle move is executed and an error is detected") {
test_objs.handler.execute_idle_move();
THEN("error is noted but not handled") {
REQUIRE(!test_objs.handler.error_handled);
}
WHEN(
"idle move is executed the second time and the error is still "
"detected") {
test_objs.handler.execute_idle_move();
THEN("error is handled") {
REQUIRE(test_objs.handler.error_handled);
}
}
WHEN(
"idle move is executed the second time and but the error is no "
"longer detected") {
test_objs.handler.set_enc_idle_state(true);
test_objs.hw.set_encoder_value(0);
test_objs.handler.execute_idle_move();
THEN("error is not handled") {
REQUIRE(!test_objs.handler.error_handled);
}
}
}
}
}

SCENARIO("handler recovers from error state") {
BrushedMotorContainer test_objs{};
auto og_motor_state = GENERATE(BrushedMotorState::FORCE_CONTROLLING_HOME,
Expand Down

0 comments on commit fb65580

Please sign in to comment.