From 7002e3b0c7e9bf45cc444750a20d4a13af649772 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 29 Jul 2022 17:54:17 +0200
Subject: [PATCH] Avoid homing Idler and Selector at the same time

This PR is an experimental code to delay homing of the Selector after the Idler homes properly.

Unit tests are expected to fail at this moment...
---
 src/modules/movable_base.cpp |  4 ++--
 src/modules/movable_base.h   |  1 +
 src/modules/selector.cpp     | 13 ++++++++++++-
 3 files changed, 15 insertions(+), 3 deletions(-)

diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp
index 25099db..25f3988 100644
--- a/src/modules/movable_base.cpp
+++ b/src/modules/movable_base.cpp
@@ -15,9 +15,9 @@ void MovableBase::PlanHome() {
     mm::motion.StallGuardReset(axis);
 
     // plan move at least as long as the axis can go from one side to the other
-    PlanHomingMoveForward();
-    state = HomeForward;
+    state = HomeForward; // beware - the derived class may change the state if necessary
     currentSlot = -1; // important - other state machines may be waiting for a valid Slot() which is not yet correct while homing in progress
+    PlanHomingMoveForward();
 }
 
 MovableBase::OperationResult MovableBase::InitMovement() {
diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h
index 990f1e1..f95794c 100644
--- a/src/modules/movable_base.h
+++ b/src/modules/movable_base.h
@@ -14,6 +14,7 @@ public:
     enum {
         Ready = 0, // intentionally set as zero in order to allow zeroing the Idler structure upon startup -> avoid explicit initialization code
         Moving,
+        PlannedHome,
         HomeForward,
         HomeBack,
         TMCFailed,
diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
index 14b5251..5d55ac9 100644
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@@ -7,6 +7,7 @@
 #include "permanent_storage.h"
 #include "../debug.h"
 #include "globals.h"
+#include "idler.h" // @@TODO this is not nice - introduces dependency between the idler and selector - presumably electrical reasons :(
 
 namespace modules {
 namespace selector {
@@ -19,7 +20,7 @@ void Selector::PrepareMoveToPlannedSlot() {
 }
 
 void Selector::PlanHomingMoveForward() {
-    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+    state = PlannedHome;
     dbg_logic_P(PSTR("Plan Homing Selector Forward"));
 }
 
@@ -93,6 +94,16 @@ bool Selector::Step() {
         PerformMove();
         //dbg_logic_P(PSTR("Moving Selector"));
         return false;
+    case PlannedHome:
+        // A testing workaround for presumed electrical reasons why the Idler and Selector cannot perform reliable homing together.
+        // Let's wait for the Idler to finish homing before homing the selector.
+        // This will surely break the unit tests, but that's not the point at this stage.
+        if (mi::idler.HomingValid()) {
+            // idler is ok, we can start homing the selector
+            state = HomeForward;
+            mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+        }
+        return false;
     case HomeForward:
         dbg_logic_P(PSTR("Homing Selector Forward"));
         PerformHomeForward();