Fix MMU3 fsensor handling

src/logic/unload_to_finda.cpp

@@ -10,18 +10,27 @@
 #include "../modules/pulley.h"
 #include "../modules/timebase.h"
 
+namespace {
+  // Local parameters; if you prefer, move them to config::
+  constexpr uint16_t kFsensorWaitTimeout_ms = 4000; // how long we wait for fsensor==OFF before micro-pull
+  constexpr uint8_t  kMaxMicroPullTries     = 2;    // number of 1mm micro-pull attempts
+  constexpr unit::U_mm kMicroPull_mm          = 1.0_mm;  // distance of each micro-pull
+}
+
 namespace logic {
 
 void UnloadToFinda::Reset(uint8_t maxTries) {
   this->maxTries = maxTries;
   // check the inital state of FINDA and plan the moves
   if (!mf::finda.Pressed()) {
-        state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded
+    state = OK; // FINDA is already off, we assume the filament is not there, i.e. already unloaded
   } else {
     // FINDA is sensing the filament, plan moves to unload it
     state = EngagingIdler;
-        mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // basically prepare before the active slot - saves ~1s
-        started_ms = mt::timebase.Millis();
+    mi::idler.PartiallyDisengage(mg::globals.ActiveSlot()); // prepare before the active slot - saves ~1s
+    started_ms = 0;               // start timeout later, when we actually wait for fsensor==OFF
+    microPullTries = 0;
+    microPullMovePlanned = false;
     ml::leds.ActiveSlotProcessing();
   }
 }
@@ -29,17 +38,22 @@ void UnloadToFinda::Reset(uint8_t maxTries) {
 bool UnloadToFinda::Step() {
   switch (state) {
   // start by engaging the idler into intermediate position
-    // Then, wait for !fsensor.Pressed: that's to speed-up the pull process - unload operation will be started during the purging moves
+  // Then, wait for !fsensor.Pressed: to speed-up the pull process - unload operation will be started during the purging moves
   // and as soon as the fsensor turns off, the MMU engages the idler fully and starts pulling.
   // 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 (!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
+      // Start the timeout NOW (not in Reset), only when we actually wait for fsensor==OFF
+      if (started_ms == 0) started_ms = mt::timebase.Millis();
+      // If timeout elapsed, trigger micro-pull strategy with idler fully engaged
+      if (mt::timebase.Elapsed(started_ms, kFsensorWaitTimeout_ms)) {
+        mpu::pulley.InitAxis();
+        mi::idler.Engage(mg::globals.ActiveSlot());
+        microPullMovePlanned = false; // plan in MicroPullTry
+        state = MicroPullTry;
       }
       return false;
     } else {
@@ -48,14 +62,56 @@ bool UnloadToFinda::Step() {
         state = UnloadingToFinda;
         mpu::pulley.InitAxis();
         mi::idler.Engage(mg::globals.ActiveSlot());
-
         // slow move for the first few millimeters - help the printer relieve the filament while engaging the Idler fully
-                mpu::pulley.PlanMove(-config::fsensorToNozzleAvoidGrindUnload, mg::globals.PulleySlowFeedrate_mm_s(), mg::globals.PulleySlowFeedrate_mm_s());
+        mpu::pulley.PlanMove(-config::fsensorToNozzleAvoidGrindUnload,
+                             mg::globals.PulleySlowFeedrate_mm_s(),
+                             mg::globals.PulleySlowFeedrate_mm_s());
       } else {
         state = FailedFINDA;
       }
     }
     return false;
+
+  case MicroPullTry:
+    // Ensure the Idler is fully engaged before moving
+    if (!mi::idler.Engaged()) {
+      return false;
+    }
+    // Plan the 1mm micro-pull only once per attempt
+    if (!microPullMovePlanned) {
+      mpu::pulley.PlanMove(-kMicroPull_mm,
+                           mg::globals.PulleySlowFeedrate_mm_s(),
+                           mg::globals.PulleySlowFeedrate_mm_s());
+      microPullMovePlanned = true;
+      return false;
+    }
+    // Wait for the micro move to finish
+    if (!mm::motion.QueueEmpty()) {
+      return false;
+    }
+    // After the micro-pull, check FSensor
+    if (!mfs::fsensor.Pressed()) {
+      // FSensor finally OFF -> proceed with normal unload
+      if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
+        state = UnloadingToFinda;
+        // The UnloadingToFinda state will plan the long unload move
+      } else {
+        state = FailedFINDA;
+      }
+      microPullMovePlanned = false;
+      return false;
+    }
+    // Still ON -> try again up to kMaxMicroPullTries
+    if (++microPullTries < kMaxMicroPullTries) {
+      microPullMovePlanned = false; // schedule another micro-pull
+      return false;
+    } else {
+      // attempts exhausted -> fail safely
+      state = FailedFSensor;
+      microPullMovePlanned = false;
+      return true;
+    }
+
   case UnloadingToFinda:
     if (mi::idler.Engaged()) {
       state = WaitingForFINDA;
@@ -67,9 +123,11 @@ bool UnloadToFinda::Step() {
       // - a faster unload (shorter than the specified bowden length) will be interrupted by FINDA turning off
       // - if FINDA is misaligned or faulty, the only issue will be, that the filament will be thrown behind the pulley
       //   which could have happened with the previous implementation as well, because default bowden length was set to 42cm
-            mpu::pulley.PlanMove(-config::maximumBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU, mg::globals.PulleyUnloadFeedrate_mm_s());
+      mpu::pulley.PlanMove(-config::maximumBowdenLength - config::feedToFinda - config::filamentMinLoadedToMMU,
+                           mg::globals.PulleyUnloadFeedrate_mm_s());
     }
     return false;
+
   case WaitingForFINDA: {
     int32_t currentPulley_mm = mpu::pulley.CurrentPosition_mm();
     if ((abs(unloadStart_mm - currentPulley_mm) > mm::truncatedUnit(mg::globals.FSensorUnloadCheck_mm())) && mfs::fsensor.Pressed()) {
@@ -97,6 +155,7 @@ bool UnloadToFinda::Step() {
     }
   }
   return false;
+
   case OK:
   case FailedFINDA:
   case FailedFSensor:

src/logic/unload_to_finda.h

@@ -3,7 +3,6 @@
 #include <stdint.h>
 
 namespace logic {
-
 /// @brief Unload to FINDA "small" state machine
 ///
 /// "small" state machines will serve as building blocks for high-level commands/operations
@@ -15,17 +14,21 @@ struct UnloadToFinda {
   /// internal states of the state machine
   enum : uint8_t {
     EngagingIdler,
+    MicroPullTry,
     UnloadingToFinda,
     WaitingForFINDA,
     OK,
     FailedFINDA,
     FailedFSensor
   };
+
   inline constexpr UnloadToFinda()
     : state(OK)
     , maxTries(3)
     , unloadStart_mm(0)
-        , started_ms(0) {}
+    , started_ms(0)
+    , microPullTries(0)
+    , microPullMovePlanned(false) {}
 
   /// Restart the automaton
   /// @param maxTries maximum number of retried attempts before reporting a fail
@@ -40,8 +43,10 @@ struct UnloadToFinda {
 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
+  int32_t  unloadStart_mm;     // intentionally trying to avoid using U_mm because it is a float (resp. long double)
+  uint16_t started_ms;         // timeout window while actually waiting for fsensor to turn off
+  uint8_t  microPullTries;     // how many micro-pull attempts have been executed
+  bool     microPullMovePlanned; // whether the 1mm micro-pull move has been queued
 };
 
 } // namespace logic