Improve error handling of ToolChange state machines

... and fix an edge case in LoadFilament

src/logic/load_filament.cpp

@@ -1,7 +1,6 @@
 /// @file load_filament.cpp
 #include "load_filament.h"
 #include "../modules/finda.h"
-#include "../modules/fsensor.h"
 #include "../modules/globals.h"
 #include "../modules/idler.h"
 #include "../modules/leds.h"
@@ -89,10 +88,6 @@ bool LoadFilament::StepInner() {
                 // FINDA is still NOT pressed - that smells bad
                 error = ErrorCode::FINDA_DIDNT_SWITCH_ON;
                 state = ProgressCode::ERRWaitingForUser; // stand still
-            } else if (!mfs::fsensor.Pressed()) {
-                // printer's filament sensor is still NOT pressed - that smells bad
-                error = ErrorCode::FSENSOR_DIDNT_SWITCH_ON;
-                state = ProgressCode::ERRWaitingForUser; // stand still
             } else {
                 // all sensors are ok
                 ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::off);
@@ -115,7 +110,10 @@ bool LoadFilament::StepInner() {
     case ProgressCode::ERRHelpingFilament:
         if (mf::finda.Pressed()) {
             // the help was enough to press the FINDA, we are ok, continue normally
-            state = ProgressCode::FeedingToBondtech;
+            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::blink0);
+            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

src/logic/tool_change.cpp

@@ -2,6 +2,7 @@
 #include "tool_change.h"
 #include "../modules/buttons.h"
 #include "../modules/finda.h"
+#include "../modules/fsensor.h"
 #include "../modules/globals.h"
 #include "../modules/idler.h"
 #include "../modules/leds.h"
@@ -103,10 +104,22 @@ bool ToolChange::StepInner() {
             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);
+            // we should check the state of all the sensors and either report another error or confirm the correct state
-            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
+            if (!mf::finda.Pressed()) {
-            //                mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants
+                // FINDA is still NOT pressed - that smells bad
-            state = ProgressCode::AvoidingGrind;
+                error = ErrorCode::FINDA_DIDNT_SWITCH_ON;
+                state = ProgressCode::ERRWaitingForUser; // stand still
+            } else if (!mfs::fsensor.Pressed()) {
+                // printer's filament sensor is still NOT pressed - that smells bad
+                error = ErrorCode::FSENSOR_DIDNT_SWITCH_ON;
+                state = ProgressCode::ERRWaitingForUser; // stand still
+            } else {
+                // all sensors are ok
+                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;
+            }
             break;
         default: // no event, continue waiting for user input
             break;
@@ -116,16 +129,23 @@ bool ToolChange::StepInner() {
     case ProgressCode::ERREngagingIdler:
         if (mi::idler.Engaged()) {
             state = ProgressCode::ERRHelpingFilament;
-            mm::motion.PlanMove(mm::Pulley, 450, 5000); //@@TODO constants
+            mm::motion.PlanMove<mm::Pulley>(config::pulleyHelperMove, config::pulleySlowFeedrate);
         }
         return false;
     case ProgressCode::ERRHelpingFilament:
+        // @@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;
             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
+            // which requires restarting James from its last stage
+            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
+            // helped a bit, but FINDA/Fsensor didn't trigger, return to the main error state
             state = ProgressCode::ERRDisengagingIdler;
         }
         return false;

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -126,7 +126,7 @@ void FailedLoadToFindaResolveHelpFindaTriggered(uint8_t slot, logic::LoadFilamen
         return lf.TopLevelState() == ProgressCode::ERRHelpingFilament; },
         5000));
 
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::off, ml::blink0, ErrorCode::RUNNING, ProgressCode::FeedingToBondtech));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
 }
 
 void FailedLoadToFindaResolveHelpFindaDidntTrigger(uint8_t slot, logic::LoadFilament &lf) {