From ffe5bc280746e786224b9164cf1a68e0436404b7 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 27 Aug 2021 09:07:20 +0200
Subject: [PATCH] Make Idler and Selector only wait for their own axis

... and fix the unit tests for this
---
 src/modules/idler.cpp                  | 8 +-------
 src/modules/motion.cpp                 | 4 ++++
 src/modules/motion.h                   | 2 +-
 src/modules/selector.cpp               | 2 +-
 tests/unit/logic/stubs/stub_motion.cpp | 4 ++++
 5 files changed, 11 insertions(+), 9 deletions(-)

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