From 7fbd3c9c7e788609f7012c905fe0e9640d6308ee Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Mon, 31 Jan 2022 08:09:57 +0100 Subject: [PATCH] Fix homing error recovery + add homing unit tests --- src/logic/command_base.cpp | 3 +- src/modules/movable_base.cpp | 2 + src/modules/movable_base.h | 6 +- tests/unit/logic/CMakeLists.txt | 2 + tests/unit/logic/helpers/helpers.ipp | 2 +- tests/unit/logic/homing/CMakeLists.txt | 42 +++++++ tests/unit/logic/homing/test_homing.cpp | 153 ++++++++++++++++++++++++ 7 files changed, 205 insertions(+), 5 deletions(-) create mode 100644 tests/unit/logic/homing/CMakeLists.txt create mode 100644 tests/unit/logic/homing/test_homing.cpp diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp index d27e385..b2512fb 100644 --- a/src/logic/command_base.cpp +++ b/src/logic/command_base.cpp @@ -90,8 +90,9 @@ bool CommandBase::WaitForOneModuleErrorRecovery(ErrorCode ec, modules::motion::M // homing can be recovered mui::Event ev = mui::userInput.ConsumeEvent(); if (ev == mui::Event::Middle) { - m.InvalidateHoming(); // @@TODO invalidate and force initiate a new homing attempt + m.PlanHome(); // force initiate a new homing attempt state = ProgressCode::Homing; + error = ErrorCode::RUNNING; } } // TMC errors cannot be recovered safely, waiting for power cycling the MMU diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index afc1b19..6addf2d 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -7,6 +7,8 @@ namespace modules { namespace motion { void MovableBase::PlanHome() { + InvalidateHoming(); + // switch to normal mode on this axis mm::motion.InitAxis(axis); mm::motion.SetMode(axis, mm::Normal); diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 442ba96..01584ae 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -59,6 +59,9 @@ public: /// (which makes homing completely transparent) inline void InvalidateHoming() { homingValid = false; } + /// Prepare a homing move of the axis + void PlanHome(); + inline bool HomingValid() const { return homingValid; } inline config::Axis Axis() const { return axis; } @@ -92,9 +95,6 @@ protected: OperationResult InitMovement(); - /// Prepare a homing move of the axis - void PlanHome(); - void PerformMove(); void PerformHomeForward(); diff --git a/tests/unit/logic/CMakeLists.txt b/tests/unit/logic/CMakeLists.txt index ae20670..db0aecb 100644 --- a/tests/unit/logic/CMakeLists.txt +++ b/tests/unit/logic/CMakeLists.txt @@ -1,5 +1,7 @@ add_subdirectory(cut_filament) +add_subdirectory(homing) + add_subdirectory(feed_to_finda) add_subdirectory(feed_to_bondtech) diff --git a/tests/unit/logic/helpers/helpers.ipp b/tests/unit/logic/helpers/helpers.ipp index c22317f..95ceb52 100644 --- a/tests/unit/logic/helpers/helpers.ipp +++ b/tests/unit/logic/helpers/helpers.ipp @@ -149,6 +149,6 @@ void PressButtonAndDebounce(SM &sm, uint8_t btnIndex){ hal::adc::SetADC(config::buttonsADCIndex, config::buttonADCLimits[btnIndex][0] + 1); while (!mb::buttons.ButtonPressed(btnIndex)) { main_loop(); - sm.StepInner(); + sm.Step(); // Inner } } diff --git a/tests/unit/logic/homing/CMakeLists.txt b/tests/unit/logic/homing/CMakeLists.txt new file mode 100644 index 0000000..29a2534 --- /dev/null +++ b/tests/unit/logic/homing/CMakeLists.txt @@ -0,0 +1,42 @@ +# define the test executable +add_executable( + homing_tests + ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp + ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp + ${CMAKE_SOURCE_DIR}/src/logic/home.cpp + ${CMAKE_SOURCE_DIR}/src/logic/load_filament.cpp + ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp + ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp + ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp + ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp + ${CMAKE_SOURCE_DIR}/src/modules/debouncer.cpp + ${CMAKE_SOURCE_DIR}/src/modules/finda.cpp + ${CMAKE_SOURCE_DIR}/src/modules/fsensor.cpp + ${CMAKE_SOURCE_DIR}/src/modules/globals.cpp + ${CMAKE_SOURCE_DIR}/src/modules/idler.cpp + ${CMAKE_SOURCE_DIR}/src/modules/leds.cpp + ${CMAKE_SOURCE_DIR}/src/modules/movable_base.cpp + ${CMAKE_SOURCE_DIR}/src/modules/permanent_storage.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulley.cpp + ${CMAKE_SOURCE_DIR}/src/modules/selector.cpp + ${CMAKE_SOURCE_DIR}/src/modules/user_input.cpp + ${CMAKE_SOURCE_DIR}/src/modules/pulse_gen.cpp + ${MODULES_STUBS_DIR}/stub_adc.cpp + ${MODULES_STUBS_DIR}/stub_eeprom.cpp + ${MODULES_STUBS_DIR}/stub_gpio.cpp + ${MODULES_STUBS_DIR}/stub_shr16.cpp + ${MODULES_STUBS_DIR}/stub_timebase.cpp + ${MODULES_STUBS_DIR}/stub_tmc2130.cpp + ${LOGIC_STUBS_DIR}/main_loop_stub.cpp + ${LOGIC_STUBS_DIR}/stub_motion.cpp + test_homing.cpp + ) + +# define required search paths +target_include_directories( + homing_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal + ${CMAKE_SOURCE_DIR}/src/logic + ) + +# tell build system about the test case +add_catch_test(homing_tests) diff --git a/tests/unit/logic/homing/test_homing.cpp b/tests/unit/logic/homing/test_homing.cpp new file mode 100644 index 0000000..8ed3708 --- /dev/null +++ b/tests/unit/logic/homing/test_homing.cpp @@ -0,0 +1,153 @@ +#include "catch2/catch.hpp" + +#include "../../../../src/modules/buttons.h" +#include "../../../../src/modules/finda.h" +#include "../../../../src/modules/fsensor.h" +#include "../../../../src/modules/globals.h" +#include "../../../../src/modules/idler.h" +#include "../../../../src/modules/leds.h" +#include "../../../../src/modules/motion.h" +#include "../../../../src/modules/permanent_storage.h" +#include "../../../../src/modules/selector.h" + +#include "../../../../src/logic/home.h" +#include "../../../../src/logic/load_filament.h" +#include "../../../../src/logic/unload_filament.h" + +#include "../../modules/stubs/stub_adc.h" + +#include "../stubs/main_loop_stub.h" +#include "../stubs/stub_motion.h" + +using Catch::Matchers::Equals; + +#include "../helpers/helpers.ipp" + +bool SuccessfulHome(uint8_t slot) { + // prepare startup conditions + ForceReinitAllAutomata(); + + // change the startup to what we need here + EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley); + + // set FINDA OFF + debounce + SetFINDAStateAndDebounce(false); + + logic::Home h; + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); + + h.Reset(0); + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::Homing)); + REQUIRE_FALSE(mi::idler.HomingValid()); + REQUIRE_FALSE(ms::selector.HomingValid()); + + SimulateIdlerAndSelectorHoming(); + + REQUIRE(WhileTopState(h, ProgressCode::Homing, 5000)); + + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); + REQUIRE(mi::idler.HomingValid()); + REQUIRE(ms::selector.HomingValid()); + + return true; +} + +TEST_CASE("homing::successful_run", "[homing]") { + for (uint8_t slot = 0; slot < config::toolCount; ++slot) { + REQUIRE(SuccessfulHome(slot)); + } +} + +bool SelectorFailedRetry() { + // prepare startup conditions + ForceReinitAllAutomata(); + + // change the startup to what we need here + EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley); + + // set FINDA OFF + debounce + SetFINDAStateAndDebounce(false); + + logic::Home h; + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); + + h.Reset(0); + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::Homing)); + REQUIRE_FALSE(mi::idler.HomingValid()); + REQUIRE_FALSE(ms::selector.HomingValid()); + + { + // do 5 steps until we trigger the simulated stallguard + for (uint8_t i = 0; i < 5; ++i) { + main_loop(); + } + + mm::TriggerStallGuard(mm::Selector); + mm::TriggerStallGuard(mm::Idler); + main_loop(); + mm::motion.StallGuardReset(mm::Selector); + mm::motion.StallGuardReset(mm::Idler); + } + // now do a correct amount of steps of each axis towards the other end + uint32_t idlerSteps = mm::unitToSteps(config::idlerLimits.lenght); + // now do LESS steps than expected to simulate something is blocking the selector + uint32_t selectorSteps = mm::unitToSteps(config::selectorLimits.lenght) + 1; + uint32_t selectorTriggerShort = std::min(idlerSteps, selectorSteps) / 2; + uint32_t maxSteps = selectorTriggerShort + 1; + { + for (uint32_t i = 0; i < maxSteps; ++i) { + main_loop(); + + if (i == selectorTriggerShort) { + mm::TriggerStallGuard(mm::Selector); + } else { + mm::motion.StallGuardReset(mm::Selector); + } + } + + // make sure the Idler finishes its homing procedure (makes further checks much easier) + for (uint32_t i = maxSteps; i < idlerSteps + 1; ++i) { + main_loop(); + if (i == idlerSteps) { + mm::TriggerStallGuard(mm::Idler); + } else { + mm::motion.StallGuardReset(mm::Idler); + } + } + + // now the Selector shall perform a move into their parking positions + while (ms::selector.State() != mm::MovableBase::HomingFailed) + main_loop(); + } + + REQUIRE(WhileTopState(h, ProgressCode::Homing, 5)); + REQUIRE(mi::idler.HomingValid()); + + // cannot check the whole environment easily, the selector's and idler's positions are elsewhere + REQUIRE(h.Error() == ErrorCode::HOMING_SELECTOR_FAILED); + REQUIRE(h.State() == ProgressCode::ERRWaitingForUser); + + // do a few steps before pushing the button + WhileTopState(h, ProgressCode::ERRWaitingForUser, 5); + + PressButtonAndDebounce(h, mb::Middle); + + // it shall start homing again + REQUIRE(h.Error() == ErrorCode::RUNNING); + REQUIRE(h.State() == ProgressCode::Homing); + REQUIRE_FALSE(ms::selector.HomingValid()); + + SimulateSelectorHoming(); + + REQUIRE(WhileTopState(h, ProgressCode::Homing, 5000)); + + REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK)); + REQUIRE(mi::idler.HomingValid()); + REQUIRE(ms::selector.HomingValid()); + + return true; +} + +TEST_CASE("homing::selector_failed_retry", "[homing]") { + REQUIRE(SelectorFailedRetry()); +}