ToolChange: don't disengage Idler after unload, but engage next slot immediately

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)) {

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,9 +84,13 @@ bool UnloadFilament::StepInner() {
                 GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); // signal unloading error
             } else {
                 state = ProgressCode::DisengagingIdler;
+                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;
     case ProgressCode::DisengagingIdler:
         if (mi::idler.Disengaged() && ms::selector.State() == ms::Selector::Ready) {

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

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;
     }