Improve homing behavior more

Split the wait for HomingValid and the event where we wait
for the Selector/Idler status to go 'Ready' into separate functions

tests/unit/logic/homing/test_homing.cpp

@@ -44,10 +44,55 @@ bool SuccessfulHome(uint8_t slot) {
     REQUIRE_FALSE(mi::idler.HomingValid());
     REQUIRE_FALSE(ms::selector.HomingValid());
 
-    SimulateIdlerAndSelectorHoming(h);
+    // Idler homes first
+    SimulateIdlerHoming(h);
 
-    REQUIRE(WhileTopState(h, ProgressCode::Homing, 5000));
+    REQUIRE_FALSE(mi::idler.HomingValid());
+    REQUIRE_FALSE(ms::selector.HomingValid());
+    REQUIRE(h.TopLevelState() == ProgressCode::Homing);
 
+    // Homing becomes valid
+    SimulateIdlerWaitForHomingValid(h);
+
+    REQUIRE(mi::idler.HomingValid());
+    REQUIRE_FALSE(ms::selector.HomingValid());
+    REQUIRE(h.TopLevelState() == ProgressCode::Homing);
+
+    // Idler returns to the parked position, and it's state is now 'Ready'
+    // This will now allow the Selector to home
+    SimulateIdlerMoveToParkingPosition(h);
+
+    REQUIRE(mi::idler.HomingValid());
+    REQUIRE(mi::idler.State() == mm::MovableBase::Ready);
+    REQUIRE_FALSE(ms::selector.HomingValid());
+    REQUIRE(h.TopLevelState() == ProgressCode::Homing);
+
+    // Selector starts homing
+    SimulateSelectorHoming(h);
+
+    REQUIRE(mi::idler.HomingValid());
+    REQUIRE(mi::idler.State() == mm::MovableBase::Ready);
+    REQUIRE_FALSE(ms::selector.HomingValid());
+    REQUIRE(h.TopLevelState() == ProgressCode::Homing);
+
+    // Homing becomes valid
+    SimulateSelectorWaitForHomingValid(h);
+
+    REQUIRE(mi::idler.HomingValid());
+    REQUIRE(mi::idler.State() == mm::MovableBase::Ready);
+    REQUIRE(ms::selector.HomingValid());
+    REQUIRE(h.TopLevelState() == ProgressCode::Homing);
+
+    // If there is a planned move, the selector moves to the parked position
+    // Once at the parked position, it's state becomes 'Ready'
+    SimulateSelectorWaitForReadyState(h);
+
+    REQUIRE(mi::idler.HomingValid());
+    REQUIRE(mi::idler.State() == mm::MovableBase::Ready);
+    REQUIRE(ms::selector.HomingValid());
+    REQUIRE(ms::selector.State() == mm::MovableBase::Ready);
+
+    // 'Homing' should change to 'OK'
     REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
     REQUIRE(mi::idler.HomingValid());
     REQUIRE(ms::selector.HomingValid());
@@ -84,11 +129,12 @@ bool SelectorFailedRetry() {
     }
 
     SimulateSelectorHoming(h);
+    SimulateSelectorWaitForHomingValid(h);
 
     REQUIRE(ms::selector.HomingValid());
 
     // Wait for the selector to return to the parked position
-    REQUIRE(WhileTopState(h, ProgressCode::Homing, selectorMoveMaxSteps));
+    SimulateSelectorWaitForReadyState(h);
 
     REQUIRE(VerifyState(h, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
     REQUIRE(mi::idler.HomingValid());

tests/unit/logic/stubs/homing.cpp

@@ -52,7 +52,7 @@ void SimulateIdlerAndSelectorHoming(logic::CommandBase &cb) {
 #else
     // sadly, it looks like we need to separate homing of idler and selector due to electrical reasons
     SimulateIdlerHoming(cb);
-    SimulateSelectorHoming(cb, true);
+    SimulateSelectorHoming(cb);
 #endif
 }
 
@@ -82,7 +82,17 @@ void SimulateIdlerHoming(logic::CommandBase &cb) {
             mm::motion.StallGuardReset(mm::Idler);
         }
     }
+}
 
+void SimulateIdlerWaitForHomingValid(logic::CommandBase &cb) {
+    // Wait for the HomingValid flag to be set
+    while (!mi::idler.HomingValid()) {
+        main_loop();
+        cb.Step();
+    }
+}
+
+void SimulateIdlerMoveToParkingPosition(logic::CommandBase &cb) {
     // now the Idler shall perform a move into their parking positions
     while (mi::idler.State() != mm::MovableBase::Ready) {
         main_loop();
@@ -90,7 +100,7 @@ void SimulateIdlerHoming(logic::CommandBase &cb) {
     }
 }
 
-void SimulateSelectorHoming(logic::CommandBase &cb, bool waitForParkedPosition) {
+void SimulateSelectorHoming(logic::CommandBase &cb) {
     // do 5 steps until we trigger the simulated StallGuard
     for (uint8_t i = 0; i < 5; ++i) {
         main_loop();
@@ -116,23 +126,29 @@ void SimulateSelectorHoming(logic::CommandBase &cb, bool waitForParkedPosition)
             mm::motion.StallGuardReset(mm::Selector);
         }
     }
+}
 
+void SimulateSelectorWaitForHomingValid(logic::CommandBase &cb) {
     // Wait for the HomingValid flag to be set
     while (!ms::selector.HomingValid()) {
         main_loop();
         cb.Step();
     }
+}
 
-    // Normally the firmware does not wait for the state to turn ready. But it can
-    // be useful to setup test cases. After the selector homing is valid, it will
-    // go to it's last planned position.
-    if (waitForParkedPosition) {
+void SimulateSelectorWaitForReadyState(logic::CommandBase &cb) {
     // now the Selector shall perform a move into their parking positions
     while (ms::selector.State() != mm::MovableBase::Ready) {
         main_loop();
         cb.Step();
     }
 }
+
+void SimulateSelectorAndIdlerWaitForReadyState(logic::CommandBase &cb) {
+    while (ms::selector.State() != ms::Selector::Ready && mi::idler.State() != mi::Idler::Ready) {
+        main_loop();
+        cb.Step();
+    }
 }
 
 bool SimulateFailedHomeSelectorPostfix(logic::CommandBase &cb) {

tests/unit/logic/stubs/homing.h

@@ -5,7 +5,15 @@ class CommandBase;
 }
 
 void SimulateIdlerHoming(logic::CommandBase &cb);
-void SimulateSelectorHoming(logic::CommandBase &cb, bool waitForParkedPosition = false);
+void SimulateIdlerMoveToParkingPosition(logic::CommandBase &cb);
+void SimulateIdlerWaitForHomingValid(logic::CommandBase &cb);
+
+void SimulateSelectorHoming(logic::CommandBase &cb);
+void SimulateSelectorWaitForReadyState(logic::CommandBase &cb);
+void SimulateSelectorWaitForHomingValid(logic::CommandBase &cb);
+
+void SimulateSelectorAndIdlerWaitForReadyState(logic::CommandBase &cb);
+
 void SimulateIdlerAndSelectorHoming(logic::CommandBase &cb);
 bool SimulateFailedHomeFirstTime(logic::CommandBase &cb);
 bool SimulateFailedHomeSelectorRepeated(logic::CommandBase &cb);

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -84,8 +84,14 @@ void HomeIdlerAndSelector() {
     mi::idler.InvalidateHoming();
     ms::selector.InvalidateHoming();
     logic::NoCommand nc; // just a dummy instance which has an empty Step()
+
     SimulateIdlerHoming(nc);
-    SimulateSelectorHoming(nc, true);
+    SimulateIdlerWaitForHomingValid(nc);
+    SimulateIdlerMoveToParkingPosition(nc);
+
+    SimulateSelectorHoming(nc);
+    SimulateSelectorWaitForHomingValid(nc);
+    SimulateSelectorWaitForReadyState(nc);
 }
 
 bool EnsureActiveSlotIndex(uint8_t slot, mg::FilamentLoadState loadState) {

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -219,7 +219,23 @@ void ToolChangeFailLoadToFindaMiddleBtn(logic::ToolChange &tc, uint8_t toSlot) {
 
     REQUIRE_FALSE(mi::idler.HomingValid());
     REQUIRE_FALSE(ms::selector.HomingValid());
-    SimulateIdlerAndSelectorHoming(tc); // failed load to FINDA = nothing blocking the selector - it can rehome
+
+    // We've entered FeedToFinda state machine
+    REQUIRE(tc.TopLevelState() == ProgressCode::FeedingToFinda);
+
+    // Idler homes first
+    SimulateIdlerHoming(tc);
+    SimulateIdlerMoveToParkingPosition(tc);
+
+    // Now home the selector
+    SimulateSelectorHoming(tc);
+
+    // Wait for Selector to return to the planned slot
+    REQUIRE(WhileCondition(
+        tc, [&](uint32_t) {
+            return tc.feed.State() == tc.feed.EngagingIdler;
+        },
+        selectorMoveMaxSteps));
 
     // @@TODO here is nasty disrepancy - FINDA is not pressed, but we pretend to have something in the Selector.
     //
@@ -333,8 +349,17 @@ void ToolChangeFailFSensorMiddleBtn(logic::ToolChange &tc, uint8_t fromSlot, uin
 
     // make FINDA trigger - Idler will rehome in this step, Selector must remain at its place
     SimulateIdlerHoming(tc);
+
+    REQUIRE_FALSE(mi::idler.HomingValid());
+    REQUIRE_FALSE(ms::selector.HomingValid());
+
+    SimulateIdlerWaitForHomingValid(tc);
+
     REQUIRE(mi::idler.HomingValid());
     REQUIRE_FALSE(ms::selector.HomingValid());
+
+    SimulateIdlerMoveToParkingPosition(tc);
+
     // now trigger the FINDA
     REQUIRE(WhileCondition(tc, std::bind(SimulateFeedToFINDA, _1, 100), 5000));
     REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, fromSlot, fromSlot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingFilament));
@@ -515,10 +540,14 @@ void ToolChangeFSENSOR_TOO_EARLY(logic::ToolChange &tc, uint8_t slot) {
         200'000UL));
 
     // still unloading, but Selector can start homing
+    SimulateSelectorHoming(tc);
+    SimulateSelectorWaitForHomingValid(tc);
 
-    // Set waitForParkedPosition to true, since the unload filament statemachine
-    // explicitly waits for (ms::selector.State() == ms::Selector::Ready)
-    SimulateSelectorHoming(tc, true);
+    // Make sure we're still in unloading state
+    REQUIRE(tc.TopLevelState() == ProgressCode::UnloadingFilament);
+
+    // The unload filament state machine explicitly waits for (ms::selector.State() == ms::Selector::Ready)
+    SimulateSelectorWaitForReadyState(tc);
 
     // wait for finishing of UnloadingFilament
     WhileTopState(tc, ProgressCode::UnloadingFilament, 5000);

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -164,7 +164,6 @@ void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) {
     REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERRWaitingForUser));
 }
 
-
 void FindaDidntTriggerResolveTryAgain(uint8_t slot, logic::UnloadFilament &uf) {
     // Stage 3 - the user has to do something
     // there are 3 options:
@@ -186,6 +185,10 @@ void FindaDidntTriggerResolveTryAgain(uint8_t slot, logic::UnloadFilament &uf) {
 
     // Assume, the Idler homed (homing is invalidated after pressing the recovery button)
     SimulateIdlerHoming(uf);
+
+    // Wait for the idler homing to become valid, and for the
+    // idler to return to 'Ready' state
+    SimulateIdlerMoveToParkingPosition(uf);
 }
 
 TEST_CASE("unload_filament::finda_didnt_trigger_resolve_try_again", "[unload_filament]") {