From 7c296d1be31b20c5175ac76292f97287bb2b8b53 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Mon, 20 Oct 2025 12:58:42 +0200
Subject: [PATCH] ToolChange: don't disengage Idler after unload, but engage
 next slot immediately

---
 src/logic/tool_change.cpp     |  2 +-
 src/logic/unload_filament.cpp | 13 ++++++++++++-
 src/logic/unload_filament.h   |  6 +++++-
 src/modules/idler.cpp         |  2 +-
 4 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp
index 810fffb..97ed9b3 100644
--- a/src/logic/tool_change.cpp
+++ b/src/logic/tool_change.cpp
@@ -39,7 +39,7 @@ bool ToolChange::Reset(uint8_t param) {
     if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
         dbg_logic_P(PSTR("Filament is loaded --> unload"));
         state = ProgressCode::UnloadingFilament;
-        unl.Reset(mg::globals.ActiveSlot());
+        unl.Reset2(mg::globals.ActiveSlot());
     } else {
         mg::globals.SetFilamentLoaded(plannedSlot, mg::FilamentLoadState::InSelector); // activate the correct slot, feed uses that
         if (feed.Reset(true, false)) {
diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp
index 28079e5..dbf5ef4 100644
--- a/src/logic/unload_filament.cpp
+++ b/src/logic/unload_filament.cpp
@@ -28,11 +28,18 @@ bool UnloadFilament::Reset(uint8_t /*param*/) {
     mpu::pulley.InitAxis();
     state = ProgressCode::UnloadingToFinda;
     error = ErrorCode::RUNNING;
+    skipDisengagingIdler = false;
     unl.Reset(maxRetries);
     ml::leds.SetAllOff();
     return true;
 }
 
+bool UnloadFilament::Reset2(uint8_t param) {
+    bool rv = Reset(param);
+    skipDisengagingIdler = true;
+    return rv;
+}
+
 void UnloadFilament::UnloadFinishedCorrectly() {
     FinishedOK();
     mpu::pulley.Disable();
@@ -77,7 +84,11 @@ bool UnloadFilament::StepInner() {
                 GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); // signal unloading error
             } else {
                 state = ProgressCode::DisengagingIdler;
-                mi::idler.Disengage();
+                if (skipDisengagingIdler && ms::selector.State() == ms::Selector::Ready) {
+                    UnloadFinishedCorrectly(); // skip disengaging the Idler - to be used inside ToolChange to speed up
+                } else {
+                    mi::idler.Disengage();
+                }
             }
         }
         return false;
diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h
index 2637e1a..e1317da 100644
--- a/src/logic/unload_filament.h
+++ b/src/logic/unload_filament.h
@@ -12,12 +12,15 @@ namespace logic {
 class UnloadFilament : public CommandBase {
 public:
     inline constexpr UnloadFilament()
-        : CommandBase() {}
+        : CommandBase()
+        , skipDisengagingIdler(false) {}
 
     /// Restart the automaton
     /// @param param is not used, always unloads from the active slot
     bool Reset(uint8_t param) override;
 
+    bool Reset2(uint8_t param);
+
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;
 
@@ -32,6 +35,7 @@ private:
     UnloadToFinda unl;
     FeedToFinda feed;
     RetractFromFinda retract;
+    bool skipDisengagingIdler;
 };
 
 /// The one and only instance of UnloadFilament state machine in the FW
diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
index 7b72f07..126c4e3 100644
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@@ -104,7 +104,7 @@ Idler::OperationResult Idler::PlanMoveInner(uint8_t slot, Operation plannedOp) {
     }
 
     // already engaged or disengaged
-    if (currentlyEngaged == plannedMove) {
+    if (currentlyEngaged == plannedMove && currentSlot == plannedSlot) {
         return OperationResult::Accepted;
     }