diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
index fe789d8..c27c0a7 100644
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@@ -51,8 +51,7 @@ bool Idler::Home() {
 bool Idler::Step() {
     switch (state) {
     case Moving:
-        if (mm::motion.QueueEmpty()) { //@@TODO this will block until all axes made their movements,
-            // not sure if that is something we want
+        if (mm::motion.QueueEmpty(mm::Idler)) {
             // move finished
             state = Ready;
         }
@@ -71,10 +70,5 @@ bool Idler::Step() {
     }
 }
 
-//hal::tmc2130::MotorParams Idler::TMCDriverParams() const {
-//    return
-
-//}
-
 } // namespace idler
 } // namespace modules
diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp
index edc6e79..ef62a74 100644
--- a/src/modules/motion.cpp
+++ b/src/modules/motion.cpp
@@ -62,6 +62,10 @@ bool Motion::QueueEmpty() const {
     return true;
 }
 
+bool Motion::QueueEmpty(config::Axis axis) const {
+    return axisData[axis].ctrl.QueueEmpty();
+}
+
 void Motion::AbortPlannedMoves(bool halt) {
     for (uint8_t i = 0; i != NUM_AXIS; ++i)
         axisData[i].ctrl.AbortPlannedMoves(halt);
diff --git a/src/modules/motion.h b/src/modules/motion.h
index 05d6f10..783226b 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -239,7 +239,7 @@ public:
 
     /// @returns true if all planned moves have been finished for one axis
     /// @param axis requested
-    bool QueueEmpty(Axis axis) const { return axisData[axis].ctrl.QueueEmpty(); }
+    bool QueueEmpty(Axis axis) const;
 
     /// @returns false if new moves can still be planned for one axis
     /// @param axis axis requested
diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
index 44bd8c7..9cb77b0 100644
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@@ -36,7 +36,7 @@ bool Selector::Home() {
 bool Selector::Step() {
     switch (state) {
     case Moving:
-        if (mm::motion.QueueEmpty()) {
+        if (mm::motion.QueueEmpty(mm::Selector)) {
             // move finished
             state = Ready;
         }
diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp
index 0dacd75..b96f4f2 100644
--- a/tests/unit/logic/stubs/stub_motion.cpp
+++ b/tests/unit/logic/stubs/stub_motion.cpp
@@ -65,6 +65,10 @@ bool Motion::QueueEmpty() const {
     return true;
 }
 
+bool Motion::QueueEmpty(Axis axis) const {
+    return axes[axis].pos == axes[axis].targetPos;
+}
+
 void Motion::AbortPlannedMoves(bool halt) {
     for (uint8_t i = 0; i < 3; ++i) {
         axes[i].targetPos = axes[i].pos; // leave the axis where it was at the time of abort