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): allow the encoder value to debounce if deviated from expected val #725

Merged
merged 3 commits into from
Sep 19, 2023
+157 −21
Merged

feat(gripper): allow the encoder value to debounce if deviated from expected val #725

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
e44fe66
allow the enc value to debounce if deviated from expected val
ahiuchingau Sep 19, 2023
0e7892a
format
ahiuchingau Sep 19, 2023
271f111
fix lint
ahiuchingau Sep 19, 2023
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
68 changes: 68 additions & 0 deletions common/tests/test_debounce.cpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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() == true) {
// 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() == true) {
// 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
Open in desktop
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
Loading