Test ToolChange error recovery by button 0 and 1

src/logic/command_base.cpp

@@ -117,6 +117,7 @@ void CommandBase::GoToErrDisengagingIdler(ErrorCode ec) {
 
 void CommandBase::GoToErrEngagingIdler() {
     state = ProgressCode::ERREngagingIdler;
+    error = ErrorCode::RUNNING;
     mi::idler.Engage(mg::globals.ActiveSlot());
 }
 

src/logic/feed_to_bondtech.cpp

@@ -19,6 +19,14 @@ void FeedToBondtech::Reset(uint8_t maxRetries) {
     mi::idler.Engage(mg::globals.ActiveSlot());
 }
 
+void logic::FeedToBondtech::GoToPushToNozzle() {
+    mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InFSensor);
+    // plan a slow move to help push filament into the nozzle
+    //@@TODO the speed in mm/s must correspond to printer's feeding speed!
+    mm::motion.PlanMove<mm::Pulley>(config::fsensorToNozzle, config::pulleySlowFeedrate);
+    state = PushingFilamentIntoNozzle;
+}
+
 bool FeedToBondtech::Step() {
     switch (state) {
     case EngagingIdler:
@@ -34,11 +42,7 @@ bool FeedToBondtech::Step() {
         //dbg_logic_P(PSTR("Feed to Bondtech --> Pushing"));
         if (mfs::fsensor.Pressed()) {
             mm::motion.AbortPlannedMoves(); // stop pushing filament
-            mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InFSensor);
+            GoToPushToNozzle();
-            // plan a slow move to help push filament into the nozzle
-            //@@TODO the speed in mm/s must correspond to printer's feeding speed!
-            mm::motion.PlanMove<mm::Pulley>(config::fsensorToNozzle, config::pulleySlowFeedrate);
-            state = PushingFilamentIntoNozzle;
         } else if (mm::motion.StallGuard(mm::Pulley)) {
             // stall guard occurred during movement - the filament got stuck
             state = Failed; // @@TODO may be even report why it failed

src/logic/feed_to_bondtech.h

@@ -36,6 +36,11 @@ struct FeedToBondtech {
     /// @returns Failed if the maximum feed length has been reached without the the printer's fsensor trigger being reported
     inline uint8_t State() const { return state; }
 
+    /// Transition to PushingFilamentIntoNozzle.
+    /// Allows jumping into the state machine from outside just to push filament slowly into the nozzle
+    /// (this principle uses tool change)
+    void GoToPushToNozzle();
+
 private:
     uint8_t state;
     uint8_t maxRetries;

src/logic/tool_change.cpp

@@ -44,6 +44,21 @@ void ToolChange::Reset(uint8_t param) {
     }
 }
 
+void logic::ToolChange::GoToFeedingToBondtech() {
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
+    james.Reset(3);
+    state = ProgressCode::FeedingToBondtech;
+    error = ErrorCode::RUNNING;
+}
+
+void logic::ToolChange::FinishedCorrectly() {
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+    ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
+    state = ProgressCode::OK;
+    error = ErrorCode::OK;
+}
+
 bool ToolChange::StepInner() {
     switch (state) {
     case ProgressCode::UnloadingFilament:
@@ -62,8 +77,7 @@ bool ToolChange::StepInner() {
             if (feed.State() == FeedToFinda::Failed) {
                 GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON); // signal loading error
             } else {
-                state = ProgressCode::FeedingToBondtech;
+                GoToFeedingToBondtech();
-                james.Reset(3);
             }
         }
         break;
@@ -72,15 +86,13 @@ bool ToolChange::StepInner() {
             if (james.State() == FeedToBondtech::Failed) {
                 GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_ON); // signal loading error
             } else {
-                state = ProgressCode::OK;
+                FinishedCorrectly();
-                error = ErrorCode::OK;
             }
         }
         break;
     case ProgressCode::OK:
         return true;
 
-    // @@TODO error handling definitely needs unifying with the LoadFilament state machine
     case ProgressCode::ERRDisengagingIdler:
         ErrDisengagingIdler();
         return false;
@@ -106,10 +118,7 @@ bool ToolChange::StepInner() {
                 state = ProgressCode::ERRWaitingForUser; // stand still
             } else {
                 // all sensors are ok
-                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+                FinishedCorrectly();
-                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
@@ -127,14 +136,13 @@ bool ToolChange::StepInner() {
         // @@TODO helping filament needs improvement - the filament should try to move forward as long as the button is pressed
         if (mf::finda.Pressed()) {
             // the help was enough to press the FINDA, we are ok, continue normally
-            state = ProgressCode::FeedingToBondtech;
+            GoToFeedingToBondtech();
-            error = ErrorCode::RUNNING;
         } else if (mfs::fsensor.Pressed()) {
             // the help was enough to press the filament sensor, we are ok, continue normally
-            // This is not correct @@TODO - when the fsensor triggers, we still need to push the filament into the nozzle/gears
+            GoToFeedingToBondtech();
+            // Beware, when the fsensor triggers, we still need to push the filament into the nozzle/gears
             // which requires restarting James from its last stage
-            state = ProgressCode::FeedingToBondtech;
+            james.GoToPushToNozzle();
-            error = ErrorCode::RUNNING;
         } else if (mm::motion.QueueEmpty()) {
             // helped a bit, but FINDA/Fsensor didn't trigger, return to the main error state
             GoToErrDisengagingIdler(ErrorCode::FSENSOR_DIDNT_SWITCH_ON);

src/logic/tool_change.h

@@ -26,6 +26,11 @@ public:
     ErrorCode Error() const override;
 
 private:
+    void GoToFeedingToBondtech();
+
+    /// Common code for a correct completion of UnloadFilament
+    void FinishedCorrectly();
+
     UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
     FeedToFinda feed;
     FeedToBondtech james; // bond ;)

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -101,12 +101,12 @@ void FailedLoadToFindaResolveHelp(uint8_t slot, logic::LoadFilament &lf) {
     // In this case we check the first option
     PressButtonAndDebounce(lf, mb::Left);
 
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERREngagingIdler));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERREngagingIdler));
 
     // Stage 4 - engage the idler
     REQUIRE(WhileTopState(lf, ProgressCode::ERREngagingIdler, idlerEngageDisengageMaxSteps));
 
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRHelpingFilament));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament));
 }
 
 void FailedLoadToFindaResolveHelpFindaTriggered(uint8_t slot, logic::LoadFilament &lf) {

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -21,13 +21,15 @@ using Catch::Matchers::Equals;
 
 #include "../helpers/helpers.ipp"
 
-void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot) {
+void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot, uint32_t triggerAt = 1000) {
     // feeding to finda
     REQUIRE(WhileCondition(
         tc,
         [&](int step) -> bool {
-        if(step == 1000){ // on 1000th step make FINDA trigger
+        if(step == triggerAt){ // on specified stepNr make FINDA trigger
             hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
+        } else if(step >= triggerAt + config::findaDebounceMs + 1){
+            REQUIRE(mf::finda.Pressed() == true);
         }
         return tc.TopLevelState() == ProgressCode::FeedingToFinda; },
         200000UL));
@@ -47,7 +49,13 @@ void FeedingToBondtech(logic::ToolChange &tc, uint8_t toSlot) {
     REQUIRE(VerifyState(tc, mg::FilamentLoadState::InNozzle, mi::Idler::IdleSlotIndex(), toSlot, true, false, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK));
 }
 
-void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
+void CheckFinishedCorrectly(logic::ToolChange &tc, uint8_t toSlot) {
+    REQUIRE(tc.TopLevelState() == ProgressCode::OK);
+    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle);
+    REQUIRE(mg::globals.ActiveSlot() == toSlot);
+}
+
+void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
     EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
@@ -63,20 +71,23 @@ void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
         }
         return tc.TopLevelState() == ProgressCode::UnloadingFilament; },
         200000UL));
-    CHECKED_ELSE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::AtPulley) {
+
-        ++toSlot;
+    auto gf = ml::leds.Mode(fromSlot, ml::green);
-    }
+    auto gt = ml::leds.Mode(toSlot, ml::green);
+    auto rf = ml::leds.Mode(fromSlot, ml::red);
+    auto rt = ml::leds.Mode(toSlot, ml::red);
+
+    //    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));
 
     FeedingToFinda(tc, toSlot);
 
     FeedingToBondtech(tc, toSlot);
 
-    REQUIRE(tc.TopLevelState() == ProgressCode::OK);
+    CheckFinishedCorrectly(tc, toSlot);
-    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle);
-    REQUIRE(mg::globals.ActiveSlot() == toSlot);
 }
 
-void NoToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
+void NoToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
     // the filament is LOADED
@@ -92,7 +103,7 @@ void NoToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
     REQUIRE(tc.Error() == ErrorCode::OK);
 }
 
-void JustLoadFilament(logic::ToolChange tc, uint8_t slot) {
+void JustLoadFilament(logic::ToolChange &tc, uint8_t slot) {
     ForceReinitAllAutomata();
 
     EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley);
@@ -107,9 +118,7 @@ void JustLoadFilament(logic::ToolChange tc, uint8_t slot) {
 
     FeedingToBondtech(tc, slot);
 
-    REQUIRE(tc.TopLevelState() == ProgressCode::OK);
+    CheckFinishedCorrectly(tc, slot);
-    REQUIRE(mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle);
-    REQUIRE(mg::globals.ActiveSlot() == slot);
 }
 
 TEST_CASE("tool_change::test0", "[tool_change]") {
@@ -154,3 +163,116 @@ TEST_CASE("tool_change::same_slot_just_unloaded_filament", "[tool_change]") {
         JustLoadFilament(tc, toSlot);
     }
 }
+
+void ToolChangeFailLoadToFinda(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
+    ForceReinitAllAutomata();
+
+    EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
+
+    // restart the automaton
+    tc.Reset(toSlot);
+
+    REQUIRE(WhileCondition(
+        tc,
+        [&](int step) -> bool {
+        if(step == 2000){ // 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);
+
+    // feeding to finda, but fails - do not trigger FINDA
+    REQUIRE(WhileTopState(tc, ProgressCode::FeedingToFinda, 50000UL));
+
+    // should end up in error disengage idler
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_ON, ProgressCode::ERRDisengagingIdler));
+    REQUIRE(WhileTopState(tc, ProgressCode::ERRDisengagingIdler, 5000));
+}
+
+void ToolChangeFailLoadToFindaButton0(logic::ToolChange &tc, uint8_t toSlot) {
+    // now waiting for user input
+    PressButtonAndDebounce(tc, 0);
+
+    REQUIRE(WhileTopState(tc, ProgressCode::ERREngagingIdler, 5000UL));
+
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, false, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament));
+
+    // try push more, if FINDA triggers, continue loading
+    REQUIRE(WhileCondition(
+        tc,
+        [&](int step) -> bool {
+        if(step == 20){ // on 20th step make FINDA trigger
+            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
+        }
+        return tc.TopLevelState() == ProgressCode::ERRHelpingFilament; },
+        2000UL));
+
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::InSelector, toSlot, toSlot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToBondtech));
+
+    FeedingToBondtech(tc, toSlot);
+
+    CheckFinishedCorrectly(tc, toSlot);
+}
+
+void ToolChangeFailLoadToFindaButton1(logic::ToolChange &tc, uint8_t toSlot) {
+    // now waiting for user input
+    PressButtonAndDebounce(tc, 1);
+
+    REQUIRE(WhileCondition(
+        tc,
+        [&](int step) -> bool {
+        if(step == 2000){ // on 2000th step make FINDA trigger
+            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low);
+        }
+        return tc.TopLevelState() == ProgressCode::UnloadingFilament; },
+        200000UL));
+    REQUIRE(VerifyState(tc, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), toSlot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
+
+    // retry the whole operation
+    // beware - the FeedToFinda state machine will leverage the already engaged Idler,
+    // so the necessary number of steps to reach the FINDA is quite low (~200 was lowest once tested)
+    // without running short of max distance of Pulley to travel
+    FeedingToFinda(tc, toSlot, 200);
+
+    FeedingToBondtech(tc, toSlot);
+
+    CheckFinishedCorrectly(tc, toSlot);
+}
+
+void ToolChangeFailLoadToFindaButton2(logic::ToolChange &tc, uint8_t toSlot) {
+    // now waiting for user input
+    PressButtonAndDebounce(tc, 2);
+
+    // the user resolved the situation by hand
+    FeedingToFinda(tc, toSlot);
+
+    FeedingToBondtech(tc, toSlot);
+
+    CheckFinishedCorrectly(tc, toSlot);
+}
+
+TEST_CASE("tool_change::load_fail_FINDA_resolve_btn0", "[tool_change]") {
+    logic::ToolChange tc;
+    for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) {
+        for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) {
+            if (fromSlot != toSlot) {
+                ToolChangeFailLoadToFinda(tc, fromSlot, toSlot);
+                ToolChangeFailLoadToFindaButton0(tc, toSlot);
+            }
+        }
+    }
+}
+
+TEST_CASE("tool_change::load_fail_FINDA_resolve_btn1", "[tool_change]") {
+    logic::ToolChange tc;
+    for (uint8_t fromSlot = 0; fromSlot < config::toolCount; ++fromSlot) {
+        for (uint8_t toSlot = 0; toSlot < config::toolCount; ++toSlot) {
+            if (fromSlot != toSlot) {
+                ToolChangeFailLoadToFinda(tc, fromSlot, toSlot);
+                ToolChangeFailLoadToFindaButton1(tc, toSlot);
+            }
+        }
+    }
+}

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -164,7 +164,7 @@ void FindaDidntTriggerResolveHelp(uint8_t slot, logic::UnloadFilament &uf) {
     // no change in selector's position
     // FINDA still on
     // red LED should blink, green LED should be off
-    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERREngagingIdler));
+    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, true, false, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERREngagingIdler));
 
     // Stage 4 - engage the idler
     REQUIRE(WhileTopState(uf, ProgressCode::ERREngagingIdler, idlerEngageDisengageMaxSteps));
@@ -174,7 +174,7 @@ void FindaDidntTriggerResolveHelp(uint8_t slot, logic::UnloadFilament &uf) {
     // no change in selector's position
     // FINDA still on
     // red LED should blink, green LED should be off
-    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::FINDA_DIDNT_SWITCH_OFF, ProgressCode::ERRHelpingFilament));
+    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::ERRHelpingFilament));
 }
 
 void FindaDidntTriggerResolveHelpFindaTriggered(uint8_t slot, logic::UnloadFilament &uf) {