diff --git a/src/logic/feed_to_finda.cpp b/src/logic/feed_to_finda.cpp
index 165c294..ffa73a2 100644
--- a/src/logic/feed_to_finda.cpp
+++ b/src/logic/feed_to_finda.cpp
@@ -3,6 +3,7 @@
 #include "../modules/finda.h"
 #include "../modules/globals.h"
 #include "../modules/idler.h"
+#include "../modules/selector.h"
 #include "../modules/leds.h"
 #include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
@@ -15,20 +16,23 @@ namespace mi = modules::idler;
 namespace ml = modules::leds;
 namespace mb = modules::buttons;
 namespace mg = modules::globals;
+namespace ms = modules::selector;
 
 void FeedToFinda::Reset(bool feedPhaseLimited) {
     state = EngagingIdler;
     this->feedPhaseLimited = feedPhaseLimited;
     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());
 }
 
 bool FeedToFinda::Step() {
     switch (state) {
     case EngagingIdler:
-        if (mi::idler.Engaged()) {
+        if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
             state = PushingFilament;
             ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
-            mm::motion.PlanMove(feedPhaseLimited ? 1500 : 65535, 0, 0, 4000, 0, 0); //@@TODO constants
+            mm::motion.PlanMove(feedPhaseLimited ? 1500 : 32767, 0, 0, 4000, 0, 0); //@@TODO constants
         }
         return false;
     case PushingFilament:
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index 787eca8..f79b6b0 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -14,7 +14,9 @@ bool Motion::StallGuard(Axis axis) { return false; }
 
 void Motion::ClearStallGuardFlag(Axis axis) {}
 
-void Motion::PlanMove(uint16_t pulley, uint16_t idler, uint16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {}
+void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {}
+
+void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {}
 
 void Motion::Home(Axis axis, bool direction) {}
 
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 3da0418..865742b 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -76,7 +76,13 @@ public:
 
     /// Enqueue move of a specific motor/axis into planner buffer
     /// @param pulley, idler, selector - target coords
-    void PlanMove(uint16_t pulley, uint16_t idler, uint16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed);
+    void PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed);
+
+    /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate
+    /// @param axis axis affected
+    /// @param delta number of steps in either direction
+    /// @param feedrate maximum feedrate/speed after acceleration
+    void PlanMove(Axis axis, int16_t delta, uint16_t feedrate);
 
     /// Enqueue performing of homing of an axis
     /// @@TODO
diff --git a/tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp b/tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp
index 2325b10..27b6555 100644
--- a/tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp
+++ b/tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp
@@ -30,13 +30,22 @@ bool WhileCondition(logic::FeedToFinda &ff, COND cond, uint32_t maxLoops = 5000)
 
 TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
     using namespace logic;
-    FeedToFinda ff;
-    main_loop();
+
+    // no buttons involved ;)
+    hal::adc::TADCData noButtons({ 0 });
+    hal::adc::ReinitADC(0, std::move(noButtons), 1);
+
+    // finda OFF
+    hal::adc::TADCData findaOFF({ 0 });
+    hal::adc::ReinitADC(1, std::move(findaOFF), 1);
 
     // let's assume we have the filament NOT loaded and active slot 0
     modules::globals::globals.SetFilamentLoaded(false);
     modules::globals::globals.SetActiveSlot(0);
 
+    FeedToFinda ff;
+    main_loop();
+
     // restart the automaton
     ff.Reset(false);
 
@@ -46,6 +55,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
     // check if the idler and selector have the right command
     CHECK(modules::motion::axes[modules::motion::Idler].targetPos == 0); // @@TODO constants
     CHECK(modules::motion::axes[modules::motion::Selector].targetPos == 0); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Idler].enabled == true); // @@TODO constants
+    CHECK(modules::motion::axes[modules::motion::Selector].enabled == true); // @@TODO constants
 
     // engaging idler
     REQUIRE(WhileCondition(
@@ -53,15 +64,15 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
         [&]() { return !modules::idler::idler.Engaged(); },
         5000));
 
-    // idler engaged, we'll start pushing filament
+    // idler engaged, selector in position, we'll start pushing filament
     REQUIRE(ff.State() == FeedToFinda::PushingFilament);
     // at least at the beginning the LED should shine green (it should be blinking, but this mode has been already verified in the LED's unit test)
     REQUIRE(modules::leds::leds.LedOn(modules::globals::globals.ActiveSlot(), modules::leds::Color::green));
 
     // now let the filament be pushed into the FINDA - do 500 steps without triggering the condition
     // and then let the simulated ADC channel 1 create a FINDA switch
-    hal::adc::TADCData switchFindaOn({ 0, 600, 700, 800, 900 });
-    hal::adc::ReinitADC(1, std::move(switchFindaOn), 100);
+    hal::adc::TADCData switchFindaOn({ 600, 700, 800, 900 });
+    hal::adc::ReinitADC(1, std::move(switchFindaOn), 1);
     REQUIRE(WhileCondition(
         ff,
         [&]() { return ff.State() == FeedToFinda::PushingFilament; },
@@ -91,13 +102,25 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
 
 TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
     using namespace logic;
-    FeedToFinda ff;
-    main_loop();
+
+    // This is a problem - how to reset all the state machines at once?
+    // May be add an #ifdef unit_tests and a reset function for each of the automatons
+
+    // no buttons involved ;)
+    hal::adc::TADCData noButtons({ 0 });
+    hal::adc::ReinitADC(0, std::move(noButtons), 1);
+
+    // finda OFF
+    hal::adc::TADCData findaOFF({ 0 });
+    hal::adc::ReinitADC(1, std::move(findaOFF), 1);
 
     // let's assume we have the filament NOT loaded and active slot 0
     modules::globals::globals.SetFilamentLoaded(false);
     modules::globals::globals.SetActiveSlot(0);
 
+    FeedToFinda ff;
+    main_loop();
+
     // restart the automaton - we want the limited version of the feed
     ff.Reset(true);
 
diff --git a/tests/unit/logic/stubs/main_loop_stub.cpp b/tests/unit/logic/stubs/main_loop_stub.cpp
index d6d00b6..b35118d 100644
--- a/tests/unit/logic/stubs/main_loop_stub.cpp
+++ b/tests/unit/logic/stubs/main_loop_stub.cpp
@@ -5,19 +5,24 @@
 #include "../../../../src/modules/globals.h"
 #include "../../../../src/modules/idler.h"
 #include "../../../../src/modules/leds.h"
+#include "../../../../src/modules/motion.h"
 #include "../../../../src/modules/permanent_storage.h"
 #include "../../../../src/modules/selector.h"
 
 logic::CommandBase *currentCommand = nullptr;
 
 // just like in the real FW, step all the known automata
+uint16_t tmpTiming = 0;
+
 void main_loop() {
     modules::buttons::buttons.Step(hal::adc::ReadADC(0));
-    modules::leds::leds.Step(1);
-    modules::finda::finda.Step(1);
-    modules::fsensor::fsensor.Step(1);
+    modules::leds::leds.Step(tmpTiming);
+    modules::finda::finda.Step(tmpTiming);
+    modules::fsensor::fsensor.Step(tmpTiming);
     modules::idler::idler.Step();
     modules::selector::selector.Step();
+    modules::motion::motion.Step();
     if (currentCommand)
         currentCommand->Step();
+    ++tmpTiming;
 }
diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp
index 7592082..c7da652 100644
--- a/tests/unit/logic/stubs/stub_motion.cpp
+++ b/tests/unit/logic/stubs/stub_motion.cpp
@@ -5,7 +5,11 @@ namespace modules {
 namespace motion {
 
 Motion motion;
-AxisSim axes[3];
+AxisSim axes[3] = {
+    { 0, 0, false, false, false },
+    { 0, 0, false, false, false },
+    { 0, 0, false, false, false },
+};
 
 void Motion::InitAxis(Axis axis) {
     axes[axis].enabled = true;
@@ -23,13 +27,17 @@ void Motion::ClearStallGuardFlag(Axis axis) {
     axes[axis].stallGuard = false;
 }
 
-void Motion::PlanMove(uint16_t pulley, uint16_t idler, uint16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {
+void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {
     axes[Pulley].targetPos = axes[Pulley].pos + pulley;
-    axes[Idler].targetPos = axes[Idler].pos + pulley;
-    axes[Selector].targetPos = axes[Selector].pos + pulley;
+    axes[Idler].targetPos = axes[Idler].pos + idler;
+    axes[Selector].targetPos = axes[Selector].pos + selector;
     // speeds and feedrates are not simulated yet
 }
 
+void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {
+    axes[axis].targetPos = axes[axis].pos + delta;
+}
+
 void Motion::Home(Axis axis, bool direction) {
     axes[Pulley].homed = true;
 }
diff --git a/tests/unit/logic/stubs/stub_motion.h b/tests/unit/logic/stubs/stub_motion.h
index 6c94389..528b69c 100644
--- a/tests/unit/logic/stubs/stub_motion.h
+++ b/tests/unit/logic/stubs/stub_motion.h
@@ -1,11 +1,12 @@
 #pragma once
+#include <stdint.h>
 
 namespace modules {
 namespace motion {
 
 struct AxisSim {
-    uint32_t pos;
-    uint32_t targetPos;
+    int32_t pos;
+    int32_t targetPos;
     bool enabled;
     bool homed;
     bool stallGuard;
diff --git a/tests/unit/modules/stubs/stub_adc.cpp b/tests/unit/modules/stubs/stub_adc.cpp
index 6002137..36921b4 100644
--- a/tests/unit/modules/stubs/stub_adc.cpp
+++ b/tests/unit/modules/stubs/stub_adc.cpp
@@ -20,7 +20,8 @@ void ReinitADC(uint8_t channel, TADCData &&d, uint8_t ovsmpl) {
 /// ADC access routines
 uint16_t ReadADC(uint8_t adc) {
     if (!oversample) {
-        ++rdptr[adc];
+        if (rdptr[adc] != values2Return[adc].end())
+            ++rdptr[adc];
         oversample = oversampleFactor;
     } else {
         --oversample;