diff --git a/src/logic/feed_to_finda.cpp b/src/logic/feed_to_finda.cpp
index 2b1130f..83b0314 100644
--- a/src/logic/feed_to_finda.cpp
+++ b/src/logic/feed_to_finda.cpp
@@ -13,6 +13,7 @@ namespace logic {
 void FeedToFinda::Reset(bool feedPhaseLimited) {
     state = EngagingIdler;
     this->feedPhaseLimited = feedPhaseLimited;
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::Mode::blink0);
     mi::idler.Engage(mg::globals.ActiveSlot());
     // We can't get any FINDA readings if the selector is at the wrong spot - move it accordingly if necessary
     ms::selector.MoveToSlot(mg::globals.ActiveSlot());
@@ -23,8 +24,7 @@ bool FeedToFinda::Step() {
     case EngagingIdler:
         if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
             state = PushingFilament;
-            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
-            mm::motion.PlanMove(mm::Pulley, feedPhaseLimited ? 1500 : 32767, 4000); //@@TODO constants
+            mm::motion.PlanMove<mm::Pulley>(config::FeedToFINDA, config::pulleyFeedrate);
             mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input
         }
         return false;
@@ -33,7 +33,7 @@ bool FeedToFinda::Step() {
             mm::motion.AbortPlannedMoves(); // stop pushing filament
             // FINDA triggered - that means it works and detected the filament tip
             state = UnloadBackToPTFE;
-            mm::motion.PlanMove(mm::Pulley, -600, 4000); //@@TODO constants
+            mm::motion.PlanMove<mm::Pulley>(-config::CuttingEdgeToFINDAmidpoint, config::pulleyFeedrate);
         } else if (mm::motion.QueueEmpty()) { // all moves have been finished and FINDA didn't switch on
             state = Failed;
             // @@TODO - shall we disengage the idler?
@@ -44,14 +44,14 @@ bool FeedToFinda::Step() {
         return false;
     case UnloadBackToPTFE:
         if (mm::motion.QueueEmpty()) { // all moves have been finished
-            //            state = DisengagingIdler;
-            //            mi::idler.Disengage();
-            //        }
-            //        return false;
-            //    case DisengagingIdler:
-            //        if (!mi::idler.Engaged()) {
+            state = DisengagingIdler;
+            mi::idler.Disengage();
+            }
+        return false;
+    case DisengagingIdler:
+        if (!mi::idler.Engaged()) {
             state = OK;
-            //            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
         }
         // @@TODO FINDA must be reported as OFF again as we are pulling the filament from it - is this correct?
         return false;