Fix ToolChange operation + unit tests

src/logic/load_filament.cpp

@@ -81,11 +81,8 @@ bool LoadFilament::StepInner() {
         break;
     case ProgressCode::OK:
         return true;
-    case ProgressCode::ERRDisengagingIdler: // couldn't unload to FINDA
+    case ProgressCode::ERRDisengagingIdler: // couldn't load to FINDA
-        if (!mi::idler.Engaged()) {
+        ErrDisengagingIdler();
-            state = ProgressCode::ERRWaitingForUser;
-            mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input
-        }
         return false;
     case ProgressCode::ERRWaitingForUser: {
         // waiting for user buttons and/or a command from the printer

src/logic/tool_change.cpp

@@ -7,6 +7,7 @@
 #include "../modules/motion.h"
 #include "../modules/permanent_storage.h"
 #include "../modules/selector.h"
+#include "../modules/user_input.h"
 #ifdef DEBUG_LOGIC
 #include "../hal/usart.h"
 #include <string.h>
@@ -41,11 +42,13 @@ void ToolChange::Reset(uint8_t param) {
         state = ProgressCode::UnloadingFilament;
         unl.Reset(mg::globals.ActiveSlot());
     } else {
-        state = ProgressCode::LoadingFilament;
+        state = ProgressCode::FeedingToFinda;
+        error = ErrorCode::RUNNING;
 #ifdef DEBUG_LOGIC
         hu::usart1.puts("Filament is not loaded --> load\n");
 #endif //DEBUG_LOGIC
-        load.Reset(plannedSlot);
+        mg::globals.SetActiveSlot(plannedSlot);
+        feed.Reset(true);
     }
 }
 
@@ -56,21 +59,85 @@ 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
-            state = ProgressCode::LoadingFilament;
+            state = ProgressCode::FeedingToFinda;
-            load.Reset(plannedSlot);
+            error = ErrorCode::RUNNING;
+            feed.Reset(true);
         }
         break;
-    case ProgressCode::LoadingFilament:
+    case ProgressCode::FeedingToFinda:
-        if (load.StepInner()) {
+        if (feed.Step()) {
-            // loading sequence finished - basically, no errors can occurr here
+            if (feed.State() == FeedToFinda::Failed) {
-            // as LoadFilament should handle all the possible error states on its own
+                state = ProgressCode::ERRDisengagingIdler;
-            // There is no way the LoadFilament to finish in an error state
+                error = ErrorCode::FINDA_DIDNT_SWITCH_ON;
-            state = ProgressCode::OK;
+                mi::idler.Disengage();
-            error = ErrorCode::OK;
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0); // signal loading error
+            } else {
+                state = ProgressCode::FeedingToBondtech;
+                james.Reset(3);
+            }
+        }
+        break;
+    case ProgressCode::FeedingToBondtech:
+        if (james.Step()) {
+            if (james.State() == FeedToBondtech::Failed) {
+                state = ProgressCode::ERRDisengagingIdler;
+                error = ErrorCode::FSENSOR_DIDNT_SWITCH_ON;
+                mi::idler.Disengage();
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0); // signal loading error
+            } else {
+                mg::globals.SetFilamentLoaded(true);
+                state = ProgressCode::OK;
+                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;
+    case ProgressCode::ERRWaitingForUser: {
+        // waiting for user buttons and/or a command from the printer
+        mui::Event ev = mui::userInput.ConsumeEvent();
+        switch (ev) {
+        case mui::Event::Left: // try to manually load just a tiny bit - help the filament with the pulley
+            state = ProgressCode::ERREngagingIdler;
+            mi::idler.Engage(mg::globals.ActiveSlot());
+            break;
+        case mui::Event::Middle: // try again the whole sequence
+            Reset(mg::globals.ActiveSlot());
+            break;
+        case mui::Event::Right: // problem resolved - the user pushed the fillament by hand?
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
+            //                mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants
+            state = ProgressCode::AvoidingGrind;
+            break;
+        default: // no event, continue waiting for user input
+            break;
+        }
+        return false;
+    }
+    case ProgressCode::ERREngagingIdler:
+        if (mi::idler.Engaged()) {
+            state = ProgressCode::ERRHelpingFilament;
+            mm::motion.PlanMove(mm::Pulley, 450, 5000); //@@TODO constants
+        }
+        return false;
+    case ProgressCode::ERRHelpingFilament:
+        if (mf::finda.Pressed()) {
+            // the help was enough to press the FINDA, we are ok, continue normally
+            state = ProgressCode::FeedingToBondtech;
+            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;
+        }
+        return false;
     default: // we got into an unhandled state, better report it
         state = ProgressCode::ERRInternal;
         error = ErrorCode::INTERNAL;
@@ -83,8 +150,6 @@ ProgressCode ToolChange::State() const {
     switch (state) {
     case ProgressCode::UnloadingFilament:
         return unl.State(); // report sub-automaton states properly
-    case ProgressCode::LoadingFilament:
-        return load.State(); // report sub-automaton states properly
     default:
         return state;
     }
@@ -92,10 +157,10 @@ ProgressCode ToolChange::State() const {
 
 ErrorCode ToolChange::Error() const {
     switch (state) {
-    case ProgressCode::UnloadingFilament:
+    case ProgressCode::UnloadingFilament: {
-        return unl.Error(); // report sub-automaton errors properly
+        ErrorCode ec = unl.Error(); // report sub-automaton errors properly, only filter out OK and replace them with RUNNING
-    case ProgressCode::LoadingFilament:
+        return ec == ErrorCode::OK ? ErrorCode::RUNNING : ec;
-        return load.Error(); // report sub-automaton errors properly
+    }
     default:
         return error;
     }

src/logic/tool_change.h

@@ -2,7 +2,8 @@
 #include <stdint.h>
 #include "command_base.h"
 #include "unload_filament.h"
-#include "load_filament.h"
+#include "feed_to_finda.h"
+#include "feed_to_bondtech.h"
 
 namespace logic {
 
@@ -25,7 +26,8 @@ public:
 
 private:
     UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
-    LoadFilament load;
+    FeedToFinda feed;
+    FeedToBondtech james; // bond ;)
     uint8_t plannedSlot;
 };
 

src/logic/unload_filament.cpp

@@ -69,10 +69,7 @@ bool UnloadFilament::StepInner() {
         }
         return false;
     case ProgressCode::ERRDisengagingIdler: // couldn't unload to FINDA
-        if (!mi::idler.Engaged()) {
+        ErrDisengagingIdler();
-            state = ProgressCode::ERRWaitingForUser;
-            mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input
-        }
         return false;
     case ProgressCode::ERRWaitingForUser: {
         // waiting for user buttons and/or a command from the printer

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -21,6 +21,32 @@ using Catch::Matchers::Equals;
 
 #include "../helpers/helpers.ipp"
 
+void FeedingToFinda(logic::ToolChange &tc, uint8_t toSlot) {
+    // feeding to finda
+    REQUIRE(WhileCondition(
+        tc,
+        [&](int step) -> bool {
+        if(step == 1000){ // on 1000th step make FINDA trigger
+            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
+        }
+        return tc.TopLevelState() == ProgressCode::FeedingToFinda; },
+        200000UL));
+    REQUIRE(VerifyState(tc, false, toSlot, toSlot, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToBondtech));
+}
+
+void FeedingToBondtech(logic::ToolChange &tc, uint8_t toSlot) {
+    // james is feeding
+    REQUIRE(WhileCondition(
+        tc,
+        [&](int step) -> bool {
+        if(step == 5000){ // on 5000th step make filament sensor trigger
+            mfs::fsensor.ProcessMessage(true);
+        }
+        return tc.TopLevelState() == ProgressCode::FeedingToBondtech; },
+        200000UL));
+    REQUIRE(VerifyState(tc, true, mi::Idler::IdleSlotIndex(), toSlot, true, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK));
+}
+
 void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
@@ -39,14 +65,9 @@ void ToolChange(logic::ToolChange tc, uint8_t fromSlot, uint8_t toSlot) {
         200000UL));
     REQUIRE(mg::globals.FilamentLoaded() == false);
 
-    REQUIRE(WhileCondition(
+    FeedingToFinda(tc, toSlot);
-        tc,
+
-        [&](int step) -> bool {
+    FeedingToBondtech(tc, toSlot);
-        if(step == 1000){ // on 1000th step make FINDA trigger
-            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
-        }
-        return tc.TopLevelState() == ProgressCode::LoadingFilament; },
-        200000UL));
 
     REQUIRE(tc.TopLevelState() == ProgressCode::OK);
     REQUIRE(mg::globals.FilamentLoaded() == true);
@@ -82,14 +103,9 @@ void JustLoadFilament(logic::ToolChange tc, uint8_t slot) {
     // restart the automaton
     tc.Reset(slot);
 
-    REQUIRE(WhileCondition(
+    FeedingToFinda(tc, slot);
-        tc,
+
-        [&](int step) -> bool {
+    FeedingToBondtech(tc, slot);
-        if(step == 1000){ // on 1000th step make FINDA trigger
-            hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::high);
-        }
-        return tc.TopLevelState() == ProgressCode::LoadingFilament; },
-        200000UL));
 
     REQUIRE(tc.TopLevelState() == ProgressCode::OK);
     REQUIRE(mg::globals.FilamentLoaded() == true);