Distinguish among different error states when recovering ToolChange

Some errors need specific recovery, it seems it is no longer possible to "just" retry.

src/logic/tool_change.cpp

@@ -39,10 +39,8 @@ 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;
         mg::globals.SetFilamentLoaded(plannedSlot, mg::FilamentLoadState::InSelector); // activate the correct slot, feed uses that
         if (feed.Reset(true, false)) {
             state = ProgressCode::FeedingToFinda;
@@ -85,7 +83,6 @@ 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();
@@ -145,16 +142,27 @@ 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();
-            // 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.
+            // It looks like we need to distinguish next steps based on what happened
-            // If it is still pressed
+            switch (error) {
-            //  -> FINDA is flickering/badly tuned
+            case ErrorCode::FSENSOR_TOO_EARLY:
-            //  -> 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 {
+                break;
+            case ErrorCode::FINDA_FLICKERS:
+                // 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
                 GoToFeedingToFinda();
+                break;
+            default:
+                if (mf::finda.Pressed()) {
+                    Reset(mg::globals.ActiveSlot());
+                } else {
+                    GoToFeedingToFinda();
+                }
             }
             break;
         case mui::Event::Right: // problem resolved - the user pushed the fillament by hand?

src/logic/tool_change.h

@@ -38,10 +38,6 @@ 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

tests/unit/logic/helpers/helpers.ipp

@@ -64,17 +64,21 @@ bool VerifyEnvironmentState(mg::FilamentLoadState fls, uint8_t idlerSlotIndex, u
     for(uint8_t ledIndex = 0; ledIndex < config::toolCount; ++ledIndex){
         if( ledIndex != selectorSlotIndex ){
             // the other LEDs should be off
-            CHECKED_ELSE(ml::leds.Mode(ledIndex, ml::red) == ml::off) {
+            auto red = ml::leds.Mode(ledIndex, ml::red);
+            CHECKED_ELSE(red == ml::off) {
             return false;
             }
-            CHECKED_ELSE(ml::leds.Mode(ledIndex, ml::green) == ml::off) {
+            auto green = ml::leds.Mode(ledIndex, ml::green);
+            CHECKED_ELSE(green == ml::off) {
             return false;
             }
         } else {
-            CHECKED_ELSE(ml::leds.Mode(selectorSlotIndex, ml::red) == redLEDMode) {
+            auto red = ml::leds.Mode(selectorSlotIndex, ml::red);
+            CHECKED_ELSE(red == redLEDMode) {
             return false;
             }
-            CHECKED_ELSE(ml::leds.Mode(selectorSlotIndex, ml::green) == greenLEDMode) {
+            auto green = ml::leds.Mode(selectorSlotIndex, ml::green);
+            CHECKED_ELSE(green == greenLEDMode) {
             return false;
             }
         }

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -76,6 +76,7 @@ void ForceReinitAllAutomata() {
     mm::ReinitMotion();
 
     // let's assume we have the filament NOT loaded and active slot 0
+    mg::globals.Init();
     mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::AtPulley);
 }
 

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -64,6 +64,21 @@ void CheckFinishedCorrectly(logic::ToolChange &tc, uint8_t toSlot) {
     REQUIRE(mg::globals.ActiveSlot() == toSlot);
 }
 
+// This function exists for the sole purpose of debugging.
+// WritePin is always_inline and gdb has a hard time settinge breakpoints when FINDA should do something
+void FINDAOnOff(bool press) {
+    hal::gpio::WritePin(FINDA_PIN, press ? hal::gpio::Level::high : hal::gpio::Level::low);
+}
+
+bool SimulateUnloadFilament(uint32_t step, const logic::CommandBase *tc, uint32_t unloadLengthSteps) {
+    if (step == 20) { // on 20th step make FSensor switch off
+        mfs::fsensor.ProcessMessage(false);
+    } else if (step == unloadLengthSteps) {
+        FINDAOnOff(false);
+    }
+    return tc->TopLevelState() == ProgressCode::UnloadingFilament;
+}
+
 void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
@@ -76,14 +91,8 @@ void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
 
     REQUIRE(WhileCondition(
         tc,
-        [&](uint32_t step) -> bool {
+        std::bind(SimulateUnloadFilament, _1, &tc, mm::unitToSteps<mm::P_pos_t>(config::minimumBowdenLength)),
-        if(step == 20){ // on 20th step make FSensor switch off
+        200'000UL));
-            mfs::fsensor.ProcessMessage(false);
-        } else if(step == mm::unitToSteps<mm::P_pos_t>(config::minimumBowdenLength)){ // on 2000th step make FINDA trigger
-            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
-        }
-        return tc.TopLevelState() == ProgressCode::UnloadingFilament; },
-        200000UL));
 
     //    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley);
     REQUIRE(VerifyState2(tc, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), fromSlot, false, false, toSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
@@ -493,3 +502,65 @@ TEST_CASE("tool_change::test_flickering_FINDA_keepPressed", "[tool_change]") {
         }
     }
 }
+
+void ToolChangeFSENSOR_TOO_EARLY(logic::ToolChange &tc, uint8_t slot) {
+    ForceReinitAllAutomata();
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley));
+
+    // verify filament NOT loaded
+    REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off));
+
+    // restart the automaton
+    tc.Reset(slot);
+
+    FeedingToFinda(tc, slot);
+
+    REQUIRE_FALSE(mfs::fsensor.Pressed());
+    REQUIRE(WhileCondition(
+        tc,
+        [&](uint32_t step) -> bool {
+            if(step == mm::unitToSteps<mm::P_pos_t>(config::minimumBowdenLength) / 2){ // make FSensor trigger at the half of minimal distance
+                mfs::fsensor.ProcessMessage(true);
+            }
+            return tc.TopLevelState() == ProgressCode::FeedingToBondtech; },
+        mm::unitToSteps<mm::P_pos_t>(config::minimumBowdenLength) + 10000));
+
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRDisengagingIdler));
+    SimulateErrDisengagingIdler(tc, ErrorCode::FSENSOR_TOO_EARLY);
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, mi::idler.IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::FSENSOR_TOO_EARLY, ProgressCode::ERRWaitingForUser));
+
+    // make AutoRetry
+    PressButtonAndDebounce(tc, mb::Middle, true);
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, mi::idler.IdleSlotIndex(), slot, true, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingFilament));
+
+    SimulateIdlerHoming(tc);
+
+    // perform regular unload, just a little short (same length as above)
+    REQUIRE(WhileCondition(
+        tc,
+        [&](uint32_t step) -> bool {
+        if(step == 20){ // on 20th step make FSensor switch off
+            mfs::fsensor.ProcessMessage(false);
+        } else if(step == mm::unitToSteps<mm::P_pos_t>(config::minimumBowdenLength)/2){
+            FINDAOnOff(false);
+        }
+        return tc.unl.State() != ProgressCode::DisengagingIdler; },
+        200'000UL));
+
+    // still unloading, but Selector can start homing
+    SimulateSelectorHoming(tc);
+    // wait for finishing of UnloadingFilament
+    WhileTopState(tc, ProgressCode::UnloadingFilament, 5000);
+
+    REQUIRE(VerifyState2(tc, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, slot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
+    FeedingToFinda(tc, slot);
+    FeedingToBondtech(tc, slot);
+    CheckFinishedCorrectly(tc, slot);
+}
+
+TEST_CASE("tool_change::test_FSENSOR_TOO_EARLY", "[tool_change]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        logic::ToolChange tc;
+        ToolChangeFSENSOR_TOO_EARLY(tc, slot);
+    }
+}