From 3d32c300a34bc001bee2197a6f14ca0eedd25202 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Thu, 23 Oct 2025 13:09:46 +0200
Subject: [PATCH] UnloadToFinda: prepare Idler and once fsensor turns off,
 stall pulling

Beware: behaviour breaking change, from now on non-reworked MK4 and MK3 cannot work with the MMU
because there is no way to turn off their fsensor and trigger the unload.

Hence the version bump to 3.0.4

But, on MMU-reworked MK4 and C1 this saves 2s per toolchange
---
 src/logic/unload_to_finda.cpp                 | 26 ++++++++++++++-----
 src/logic/unload_to_finda.h                   |  4 ++-
 .../unload_to_finda/test_unload_to_finda.cpp  | 21 ++++++++++++---
 version.cmake                                 |  2 +-
 4 files changed, 42 insertions(+), 11 deletions(-)

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
     )