Unit tests: selector refused to move in ToolChange

Revealed all kinds of subtle issues (which is great). All have been fixed in this commit.

src/logic/feed_to_finda.cpp

@@ -18,9 +18,15 @@ bool FeedToFinda::Reset(bool feedPhaseLimited, bool haltAtEnd) {
     this->feedPhaseLimited = feedPhaseLimited;
     this->haltAtEnd = haltAtEnd;
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off);
-    mi::idler.Engage(mg::globals.ActiveSlot());
+    if (ms::selector.MoveToSlot(mg::globals.ActiveSlot()) != ms::Selector::OperationResult::Accepted) {
         // We can't get any FINDA readings if the selector is at the wrong spot - move it accordingly if necessary
-    return ms::selector.MoveToSlot(mg::globals.ActiveSlot()) == ms::Selector::OperationResult::Accepted;
+        // And prevent issuing any commands to the idler in such an error state
+        return false;
+    } else {
+        // Selector accepted the command, we can plan the Idler as well
+        mi::idler.Engage(mg::globals.ActiveSlot());
+        return true;
+    }
 }
 
 bool FeedToFinda::Step() {

src/logic/load_filament.cpp

@@ -47,9 +47,13 @@ void LoadFilament::ResetLimited(uint8_t param) {
 void logic::LoadFilament::Reset2(bool feedPhaseLimited) {
     state = ProgressCode::FeedingToFinda;
     error = ErrorCode::RUNNING;
-    feed.Reset(feedPhaseLimited, true);
+    if (!feed.Reset(feedPhaseLimited, true)) {
+        // selector refused to move
+        GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF);
+    } else {
         ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off);
     }
+}
 
 void logic::LoadFilament::GoToRetractingFromFinda() {
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off);

src/logic/tool_change.cpp

@@ -39,8 +39,10 @@ bool ToolChange::Reset(uint8_t param) {
     if (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector) {
         dbg_logic_P(PSTR("Filament is loaded --> unload"));
         state = ProgressCode::UnloadingFilament;
+        unloadAlreadyFinished = false;
         unl.Reset(mg::globals.ActiveSlot());
     } else {
+        unloadAlreadyFinished = true;
         if (feed.Reset(true, false)) {
             state = ProgressCode::FeedingToFinda;
             error = ErrorCode::RUNNING;
@@ -71,7 +73,9 @@ void logic::ToolChange::GoToFeedingToFinda() {
     state = ProgressCode::FeedingToFinda;
     error = ErrorCode::RUNNING;
     mg::globals.SetFilamentLoaded(plannedSlot, mg::FilamentLoadState::AtPulley);
-    feed.Reset(true, false);
+    if (!feed.Reset(true, false)) {
+        GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF);
+    }
 }
 
 bool ToolChange::StepInner() {
@@ -81,6 +85,9 @@ bool ToolChange::StepInner() {
             // unloading sequence finished - basically, no errors can occurr here
             // as UnloadFilament should handle all the possible error states on its own
             // There is no way the UnloadFilament to finish in an error state
+            unloadAlreadyFinished = true;
+            // But planning the next move can fail if Selector refuses moving to the next slot
+            // - that scenario is handled inside GoToFeedingToFinda
             GoToFeedingToFinda();
         }
         break;
@@ -138,7 +145,13 @@ bool ToolChange::StepInner() {
             // Beware: we may run into issues when FINDA or FSensor do not work correctly. Selector may rely on the presumed filament position and actually cut it accidentally when trying to rehome.
             // It is yet to be seen if something like this can actually happen.
             InvalidateHoming();
-            if (mf::finda.Pressed()) {
+            // This is a tricky part in case FINDA is flickering
+            // If we already managed to finish the unload, we must assume finda should be NOT pressed.
+            // If it is still pressed
+            //  -> FINDA is flickering/badly tuned
+            //  -> unreliable and the user didn't fix the issue
+            //  -> we cannot do anything else but request the user to fix FINDA
+            if (mf::finda.Pressed() && (!unloadAlreadyFinished)) {
                 Reset(mg::globals.ActiveSlot());
             } else {
                 GoToFeedingToFinda();

src/logic/tool_change.h

@@ -38,6 +38,10 @@ private:
     FeedToFinda feed;
     FeedToBondtech james; // bond ;)
     uint8_t plannedSlot;
+
+    /// true if the unload phase was either not necessary or finished correctly.
+    /// Part of the flickering FINDA issue
+    bool unloadAlreadyFinished;
 };
 
 /// The one and only instance of ToolChange state machine in the FW

src/modules/idler.cpp

@@ -72,7 +72,7 @@ Idler::OperationResult Idler::Disengage() {
 
     // coordinates invalid, first home, then disengage
     if (!homingValid) {
-        PerformHomeForward();
+        PlanHome();
         return OperationResult::Accepted;
     }
 

src/modules/selector.cpp

@@ -67,6 +67,12 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
         return OperationResult::Accepted;
     }
 
+    // already at the right slot - prevent invalidating moves when already at the correct spot but FINDA is pressed
+    if (currentSlot == slot && homingValid) {
+        dbg_logic_P(PSTR("Moving Selector"));
+        return OperationResult::Accepted;
+    }
+
     if (mf::finda.Pressed()) {
         // @@TODO not sure why (if) this happens, but anyway - we must not move the selector if FINDA is pressed
         // That includes the CutFilament operation as well
@@ -79,12 +85,6 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
         return OperationResult::Accepted;
     }
 
-    // already at the right slot
-    if (currentSlot == slot) {
-        dbg_logic_P(PSTR("Moving Selector"));
-        return OperationResult::Accepted;
-    }
-
     // do the move
     return InitMovementNoReinitAxis();
 }

tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp

@@ -32,7 +32,7 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
     main_loop();
 
     // restart the automaton
-    ff.Reset(false, true);
+    REQUIRE(ff.Reset(false, true));
 
     REQUIRE(ff.State() == FeedToFinda::EngagingIdler);
 
@@ -100,7 +100,7 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
     main_loop();
 
     // restart the automaton - we want the limited version of the feed
-    ff.Reset(true, true);
+    REQUIRE(ff.Reset(true, true));
 
     REQUIRE(ff.State() == FeedToFinda::EngagingIdler);
 

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -15,6 +15,7 @@
 #include "../../../../src/logic/tool_change.h"
 
 #include "../../modules/stubs/stub_adc.h"
+#include "../../modules/stubs/stub_timebase.h"
 
 #include "../stubs/homing.h"
 #include "../stubs/main_loop_stub.h"
@@ -407,3 +408,84 @@ TEST_CASE("tool_change::load_fail_FSensor_resolve_btnM", "[tool_change]") {
         }
     }
 }
+
+void ToolChangeWithFlickeringFINDA(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot, bool keepFindaPressed) {
+    ForceReinitAllAutomata();
+
+    REQUIRE(EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle));
+    SetFINDAStateAndDebounce(true);
+    SetFSensorStateAndDebounce(true);
+
+    // restart the automaton
+    tc.Reset(toSlot);
+
+    REQUIRE(WhileCondition(tc, std::bind(SimulateUnloadToFINDA, _1, 100, 2'000), 200'000));
+
+    // This is something else than WhileTopState()==UnloadingFilament
+    // We need to catch the very moment, when the unload finished and a move to another slot is being planned
+    REQUIRE(WhileCondition(
+        tc, [&](uint32_t) -> bool { return tc.unl.State() != ProgressCode::OK; }, 5000));
+
+    // now press FINDA again, but prevent stepping other state machines
+    REQUIRE_FALSE(mf::finda.Pressed());
+    hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
+    while (!mf::finda.Pressed()) {
+        mf::finda.Step();
+        mt::IncMillis();
+    }
+    REQUIRE(mf::finda.Pressed());
+    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley);
+
+    // now what ;) - Selector cannot move, because FINDA is pressed
+    // ToolChange should emit "FINDA_DIDNT_SWITCH_OFF" and we should be able to resolve the error by Retrying
+    main_loop();
+    tc.Step();
+
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRDisengagingIdler));
+    SimulateErrDisengagingIdler(tc, ErrorCode::FINDA_DIDNT_SWITCH_OFF); // this should be a single step, Idler should remain disengaged due to previous error
+
+    // now we have 2 options what can happen:
+    // FINDA is still pressed - the user didn't manage to fix the issue
+    // FINDA is not pressed - the print should continue
+    if (keepFindaPressed) {
+        // now waiting for user input
+        REQUIRE_FALSE(mui::userInput.AnyEvent());
+        REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERRWaitingForUser));
+        PressButtonAndDebounce(tc, mb::Middle, true);
+        // we should remain in the same error state
+        REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRDisengagingIdler));
+
+        // Idler will try to rehome, allow it
+        SimulateIdlerHoming(tc);
+        REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERRWaitingForUser));
+
+        // now "fix" FINDA and the command shall finish correctly
+        SetFINDAStateAndDebounce(false);
+        ToolChangeFailLoadToFindaMiddleBtn(tc, toSlot);
+    } else {
+        SetFINDAStateAndDebounce(false);
+        ToolChangeFailLoadToFindaMiddleBtn(tc, toSlot);
+    }
+}
+
+TEST_CASE("tool_change::test_flickering_FINDA", "[tool_change]") {
+    for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) {
+        for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) {
+            logic::ToolChange tc;
+            if (fromSlot != toSlot) {
+                ToolChangeWithFlickeringFINDA(tc, fromSlot, toSlot, false);
+            }
+        }
+    }
+}
+
+TEST_CASE("tool_change::test_flickering_FINDA_keepPressed", "[tool_change]") {
+    for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) {
+        for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) {
+            logic::ToolChange tc;
+            if (fromSlot != toSlot) {
+                ToolChangeWithFlickeringFINDA(tc, fromSlot, toSlot, true);
+            }
+        }
+    }
+}