From e44fe666e32946d2f55addaf4910ea1c46217b5b Mon Sep 17 00:00:00 2001 From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 19 Sep 2023 18:23:32 -0400 Subject: [PATCH 1/3] allow the enc value to debounce if deviated from expected val --- common/tests/test_debounce.cpp | 71 +++++++++++++++++++ include/common/core/debounce.hpp | 20 +++++- .../brushed_motor_interrupt_handler.hpp | 47 +++++++----- .../test_brushed_motor_interrupt_handler.cpp | 43 ++++++++++- 4 files changed, 160 insertions(+), 21 deletions(-) diff --git a/common/tests/test_debounce.cpp b/common/tests/test_debounce.cpp index 9df0c25f1..63ad250e1 100644 --- a/common/tests/test_debounce.cpp +++ b/common/tests/test_debounce.cpp @@ -32,3 +32,74 @@ 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); + } + } + } +} diff --git a/include/common/core/debounce.hpp b/include/common/core/debounce.hpp index 1c2f5d11e..4c540b6a0 100644 --- a/include/common/core/debounce.hpp +++ b/include/common/core/debounce.hpp @@ -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 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 8cfce6c04..cf73b17ec 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 @@ -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" @@ -146,7 +147,10 @@ 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); @@ -154,9 +158,13 @@ class BrushedMotorInterruptHandler { } } 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 @@ -241,22 +249,23 @@ 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); } @@ -264,6 +273,7 @@ class BrushedMotorInterruptHandler { hardware.reset_control(); timeout_ticks = 0; error_handled = false; + enc_errored.reset(); hardware.set_stay_enabled( static_cast(buffered_move.stay_engaged)); switch (buffered_move.stop_condition) { @@ -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 diff --git a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp index 793af429b..7b2cbbf3a 100644 --- a/motor-control/tests/test_brushed_motor_interrupt_handler.cpp +++ b/motor-control/tests/test_brushed_motor_interrupt_handler.cpp @@ -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 @@ -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, From 0e7892a03b3dd4b7489616c3d71f97eaa0b665db Mon Sep 17 00:00:00 2001 From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 19 Sep 2023 18:30:43 -0400 Subject: [PATCH 2/3] format --- common/tests/test_debounce.cpp | 9 +++------ include/common/core/debounce.hpp | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/common/tests/test_debounce.cpp b/common/tests/test_debounce.cpp index 63ad250e1..bf4fde690 100644 --- a/common/tests/test_debounce.cpp +++ b/common/tests/test_debounce.cpp @@ -33,7 +33,6 @@ SCENARIO("debouncing gpio pins works") { } } - SCENARIO("debouncing with holdoff cnt works") { GIVEN("a state value and bounce value") { debouncer::Debouncer subject{.holdoff_cnt = 2}; @@ -48,9 +47,7 @@ SCENARIO("debouncing with holdoff cnt works") { subject.debounce_update(true); THEN("state updates on the third tick") { REQUIRE(subject.debounce_state() == true); - THEN("cnt gets reset") { - CHECK(subject.cnt == 0); - } + THEN("cnt gets reset") { CHECK(subject.cnt == 0); } } WHEN("reset is called") { subject.reset(); @@ -72,11 +69,11 @@ SCENARIO("debouncing with holdoff cnt works") { subject.debounce_update(false); subject.debounce_update(false); THEN("state stays the same on the first two ticks") { - REQUIRE(subject.debounce_state() == true); + REQUIRE(subject.debounce_state() == true); } subject.debounce_update(false); THEN("state updates on the third tick") { - REQUIRE(subject.debounce_state() == false); + REQUIRE(subject.debounce_state() == false); } } WHEN("switching from set to unset before holdoff cnt is reached") { diff --git a/include/common/core/debounce.hpp b/include/common/core/debounce.hpp index 4c540b6a0..2efc9f2cf 100644 --- a/include/common/core/debounce.hpp +++ b/include/common/core/debounce.hpp @@ -15,7 +15,7 @@ struct Debouncer { // will and we can set it to the new state. if (new_state == state_bounce) { - cnt ++; + cnt++; if (cnt == holdoff_cnt) { // update state only when cnt reaches the holdoff count state = new_state; From 271f111a8015df50291d6fb5cf406494a311b7c4 Mon Sep 17 00:00:00 2001 From: ahiuchingau <20424172+ahiuchingau@users.noreply.github.com> Date: Tue, 19 Sep 2023 18:55:09 -0400 Subject: [PATCH 3/3] fix lint --- .../core/brushed_motor/brushed_motor_interrupt_handler.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 cf73b17ec..5fabf2b0a 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 @@ -149,7 +149,7 @@ class BrushedMotorInterruptHandler { // movements that occur from motion and vibration enc_errored.debounce_update( move_delta > error_conf.unwanted_movement_threshold); - if (enc_errored.debounce_state() == true) { + if (enc_errored.debounce_state()) { // enc value errored for 3 consecutive ticks cancel_and_clear_moves( can::ids::ErrorCode::collision_detected); @@ -163,7 +163,7 @@ class BrushedMotorInterruptHandler { std::abs(pulses - hold_encoder_position) > error_conf.unwanted_movement_threshold); - if (enc_errored.debounce_state() == true) { + if (enc_errored.debounce_state()) { // enc value errored for 3 consecutive ticks // we have likely dropped a labware or had a collision auto err =