From 5cfb0887d00f4c6d67b02a4f69af740f042c40f6 Mon Sep 17 00:00:00 2001
From: 3d-gussner <3d.gussner@gmail.com>
Date: Wed, 29 Sep 2021 20:11:29 +0200
Subject: [PATCH] Disengage Idler after retracted from FINDA Add some dbg_logic
 output

---
 src/logic/retract_from_finda.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/src/logic/retract_from_finda.cpp b/src/logic/retract_from_finda.cpp
index 81e3609..034728b 100644
--- a/src/logic/retract_from_finda.cpp
+++ b/src/logic/retract_from_finda.cpp
@@ -4,10 +4,12 @@
 #include "../modules/idler.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
+#include "../debug.h"
 
 namespace logic {
 
 void RetractFromFinda::Reset() {
+    dbg_logic_P(PSTR("\nRetract from FINDA\n\n"));
     state = EngagingIdler;
     ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
     mi::idler.Engage(mg::globals.ActiveSlot());
@@ -17,21 +19,29 @@ bool RetractFromFinda::Step() {
     switch (state) {
     case EngagingIdler:
         if (mi::idler.Engaged()) {
+            dbg_logic_sprintf_P(PSTR("Pulley start steps %u"), mm::motion.CurPosition(mm::Pulley));
             state = UnloadBackToPTFE;
             mm::motion.PlanMove<mm::Pulley>(-config::cuttingEdgeToFindaMidpoint, config::pulleyFeedrate);
         }
         return false;
     case UnloadBackToPTFE:
+        dbg_logic_P(PSTR("Unload back to PTFE --> Pulling"));
         if (mm::motion.QueueEmpty()) { // all moves have been finished
             if (!mf::finda.Pressed()) { // FINDA switched off correctly while the move was performed
                 state = OK;
                 mg::globals.SetFilamentLoaded(mg::FilamentLoadState::AtPulley);
+                dbg_logic_sprintf_P(PSTR("Pulley end steps %u"), mm::motion.CurPosition(mm::Pulley));
+                dbg_logic_P(PSTR("Retract from FINDA --> DisengagingIdler"));
+                mi::idler.Disengage();
             } else { // FINDA didn't switch off
                 state = Failed;
                 ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
                 ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0);
             }
         }
+        if (!mi::idler.Engaged()) {
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
+        }
         return false;
     case OK:
     case Failed: