Cleanup error recovery of LoadFilament

+ cover all paths with unit tests

src/logic/load_filament.cpp

@@ -19,14 +19,26 @@ void LoadFilament::Reset(uint8_t param) {
         return;
     }
     dbg_logic_P(PSTR("Load Filament"));
+    mg::globals.SetFilamentLoaded(param, mg::FilamentLoadState::AtPulley); // still at pulley, haven't moved yet
+    Reset2();
+}
+
+void logic::LoadFilament::Reset2() {
     state = ProgressCode::FeedingToFinda;
     error = ErrorCode::RUNNING;
-    mg::globals.SetFilamentLoaded(param, mg::FilamentLoadState::AtPulley); // still at pulley, haven't moved yet
     feed.Reset(true);
     ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
     ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
 }
 
+void logic::LoadFilament::GoToRetractingFromFinda() {
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
+    state = ProgressCode::RetractingFromFinda;
+    error = ErrorCode::RUNNING;
+    retract.Reset();
+}
+
 bool LoadFilament::StepInner() {
     switch (state) {
     case ProgressCode::FeedingToFinda:
@@ -80,7 +92,12 @@ bool LoadFilament::StepInner() {
             mi::idler.Engage(mg::globals.ActiveSlot());
             break;
         case mui::Event::Middle: // try again the whole sequence
-            Reset(mg::globals.ActiveSlot());
+            // however it depends on the state of FINDA - if it is on, we must perform unload first
+            if (!mf::finda.Pressed()) {
+                Reset2();
+            } else {
+                GoToRetractingFromFinda();
+            }
             break;
         case mui::Event::Right: // problem resolved - the user pushed the fillament by hand?
             // we should check the state of all the sensors and either report another error or confirm the correct state
@@ -89,11 +106,8 @@ bool LoadFilament::StepInner() {
                 error = ErrorCode::FINDA_DIDNT_SWITCH_ON;
                 state = ProgressCode::ERRWaitingForUser; // stand still
             } else {
-                // all sensors are ok
+                // all sensors are ok - pull the filament back
-                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+                GoToRetractingFromFinda();
-                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
-                state = ProgressCode::OK;
-                error = ErrorCode::OK;
             }
             break;
         default: // no event, continue waiting for user input
@@ -110,11 +124,7 @@ bool LoadFilament::StepInner() {
     case ProgressCode::ERRHelpingFilament:
         if (mf::finda.Pressed()) {
             // the help was enough to press the FINDA, we are ok, continue normally
-            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
+            GoToRetractingFromFinda();
-            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
-            state = ProgressCode::RetractingFromFinda;
-            retract.Reset();
-            error = ErrorCode::RUNNING;
         } else if (mm::motion.QueueEmpty()) {
             // helped a bit, but FINDA didn't trigger, return to the main error state
             state = ProgressCode::ERRDisengagingIdler;

src/logic/load_filament.h

@@ -21,6 +21,9 @@ public:
     bool StepInner() override;
 
 private:
+    void GoToRetractingFromFinda();
+    void Reset2();
+
     FeedToFinda feed;
     RetractFromFinda retract;
 };

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -100,7 +100,7 @@ void FailedLoadToFindaResolveHelp(uint8_t slot, logic::LoadFilament &lf) {
 
     // In this case we check the first option
 
-    // Perform press on button 1 + debounce
+    // Perform press on button 0 + debounce
     hal::adc::SetADC(config::buttonsADCIndex, config::buttonADCLimits[0][0] + 1);
     while (!mb::buttons.ButtonPressed(0)) {
         main_loop();
@@ -136,6 +136,60 @@ void FailedLoadToFindaResolveHelpFindaDidntTrigger(uint8_t slot, logic::LoadFila
     REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRDisengagingIdler));
 }
 
+void FailedLoadToFindaResolveManual(uint8_t slot, logic::LoadFilament &lf) {
+    // simulate the user fixed the issue himself
+
+    // Perform press on button 2 + debounce + switch on FINDA
+    hal::adc::SetADC(config::buttonsADCIndex, config::buttonADCLimits[2][0] + 1);
+    hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
+    while (!mb::buttons.ButtonPressed(2)) {
+        main_loop();
+        lf.Step();
+    }
+    // pulling filament back
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
+
+    // Stage 3 - retracting from finda
+    // we'll assume the finda is working correctly here
+    REQUIRE(WhileCondition(
+        lf,
+        [&](int step) -> bool {
+        if(step == 50){ // on 50th step make FINDA trigger
+            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
+        }
+        return lf.TopLevelState() == ProgressCode::RetractingFromFinda; },
+        5000));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler));
+
+    // disengaging idler
+    REQUIRE(WhileTopState(lf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
+}
+
+void FailedLoadToFindaResolveManualNoFINDA(uint8_t slot, logic::LoadFilament &lf) {
+    // Perform press on button 2 + debounce + keep FINDA OFF (i.e. the user didn't solve anything)
+    hal::adc::SetADC(config::buttonsADCIndex, config::buttonADCLimits[2][0] + 1);
+    while (!mb::buttons.ButtonPressed(2)) {
+        main_loop();
+        lf.Step();
+    }
+    // pulling filament back
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRWaitingForUser));
+}
+
+void FailedLoadToFindaResolveTryAgain(uint8_t slot, logic::LoadFilament &lf) {
+    // Perform press on button 1 + debounce
+    hal::adc::SetADC(config::buttonsADCIndex, config::buttonADCLimits[1][0] + 1);
+    while (!mb::buttons.ButtonPressed(1)) {
+        main_loop();
+        lf.Step();
+    }
+
+    // the state machine should have restarted
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
+    LoadFilamentSuccessful(slot, lf);
+}
+
 TEST_CASE("load_filament::failed_load_to_finda_0-4_resolve_help_second_ok", "[load_filament]") {
     for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
         logic::LoadFilament lf;
@@ -178,3 +232,30 @@ TEST_CASE("load_filament::state_machine_reusal", "[load_filament]") {
         }
     }
 }
+
+TEST_CASE("load_filament::failed_load_to_finda_0-4_resolve_manual", "[load_filament]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        logic::LoadFilament lf;
+        LoadFilamentCommonSetup(slot, lf);
+        FailedLoadToFinda(slot, lf);
+        FailedLoadToFindaResolveManual(slot, lf);
+    }
+}
+
+TEST_CASE("load_filament::failed_load_to_finda_0-4_resolve_manual_no_FINDA", "[load_filament]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        logic::LoadFilament lf;
+        LoadFilamentCommonSetup(slot, lf);
+        FailedLoadToFinda(slot, lf);
+        FailedLoadToFindaResolveManualNoFINDA(slot, lf);
+    }
+}
+
+TEST_CASE("load_filament::failed_load_to_finda_0-4_try_again", "[load_filament]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        logic::LoadFilament lf;
+        LoadFilamentCommonSetup(slot, lf);
+        FailedLoadToFinda(slot, lf);
+        FailedLoadToFindaResolveTryAgain(slot, lf);
+    }
+}