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(-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: