diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp index 8c32102..3c77da4 100644 --- a/src/logic/unload_to_finda.cpp +++ b/src/logic/unload_to_finda.cpp @@ -8,6 +8,7 @@ #include "../modules/motion.h" #include "../modules/permanent_storage.h" #include "../modules/pulley.h" +#include "../modules/timebase.h" namespace logic { @@ -19,7 +20,8 @@ void UnloadToFinda::Reset(uint8_t maxTries) { } else { // FINDA is sensing the filament, plan moves to unload it state = EngagingIdler; - mi::idler.Engage(mg::globals.ActiveSlot()); + mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // basically prepare before the active slot - saves ~1s + started_ms = mt::timebase.Millis(); } } @@ -31,12 +33,24 @@ bool UnloadToFinda::Step() { // It will not wait for the extruder to finish the relieve move. // However, such an approach breaks running the MMU on a non-reworked MK4/C1, which hasn't been officially supported, but possible (with some level of uncertainity). case EngagingIdler: - if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) { - state = UnloadingToFinda; - mpu::pulley.InitAxis(); - ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); + if (!mi::idler.PartiallyDisengaged()) { // just waiting for Idler to get into the target intermediate position + return false; + } + if (mfs::fsensor.Pressed()) { // still pressed, printer didn't free the filament yet + if (mt::timebase.Elapsed(started_ms, 4000)) { + state = FailedFSensor; // fsensor didn't turn off within 4 seconds, something is seriously wrong + } + return false; } else { - state = FailedFINDA; + // fsensor is OFF and Idler is partially engaged, engage the Idler fully and pull + if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) { + state = UnloadingToFinda; + mpu::pulley.InitAxis(); + ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0); + mi::idler.Engage(mg::globals.ActiveSlot()); + } else { + state = FailedFINDA; + } } return false; case UnloadingToFinda: diff --git a/src/logic/unload_to_finda.h b/src/logic/unload_to_finda.h index e6b31fd..3b63295 100644 --- a/src/logic/unload_to_finda.h +++ b/src/logic/unload_to_finda.h @@ -24,7 +24,8 @@ struct UnloadToFinda { inline constexpr UnloadToFinda() : state(OK) , maxTries(3) - , unloadStart_mm(0) {} + , unloadStart_mm(0) + , started_ms(0) {} /// Restart the automaton /// @param maxTries maximum number of retried attempts before reporting a fail @@ -40,6 +41,7 @@ private: uint8_t state; uint8_t maxTries; int32_t unloadStart_mm; // intentionally trying to avoid using U_mm because it is a float (reps. long double) + uint16_t started_ms; // timeout on fsensor turn off }; } // namespace logic diff --git a/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp b/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp index 7360f2c..2f2ddad 100644 --- a/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp +++ b/tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp @@ -44,15 +44,30 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") { // it should have instructed the selector and idler to move to slot 1 // check if the idler and selector have the right command - CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(0).v); + CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::IntermediateSlotPosition(0).v); CHECK(mm::AxisNearestTargetPos(mm::Selector) == ms::Selector::SlotPosition(0).v); - // engaging idler + // engaging idler partially + REQUIRE(WhileCondition( + ff, + [&](uint32_t) { return !mi::idler.PartiallyDisengaged(); }, + 5000)); + + // turn off fsensor - the printer freed the filament from the gears + SetFSensorStateAndDebounce(false); + + // make sure we step ff to handle turned-off fsensor + ff.Step(); + + REQUIRE(ff.State() == logic::UnloadToFinda::UnloadingToFinda); + CHECK(mm::axes[mm::Pulley].enabled == true); + CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(0).v); + + // engaging idler fully REQUIRE(WhileCondition( ff, [&](uint32_t) { return !mi::idler.Engaged(); }, 5000)); - CHECK(mm::axes[mm::Pulley].enabled == true); // now pulling the filament until finda triggers REQUIRE(ff.State() == logic::UnloadToFinda::WaitingForFINDA); diff --git a/version.cmake b/version.cmake index 05ea17c..f4b8c0b 100644 --- a/version.cmake +++ b/version.cmake @@ -10,6 +10,6 @@ set(PROJECT_VERSION_MINOR CACHE STRING "Project minor version" FORCE ) set(PROJECT_VERSION_REV - 3 + 4 CACHE STRING "Project revision" FORCE )