Fix unit tests

That includes:
- introduce pulley slow feedrate and fsensor-to-nozzle distance
  in config necessary for slowly feeding the filament from fsensor into the nozzle.
  (the constant is subject to extraction into some other config as it has to be used in the printer as well).
- update FeedToBondtech accordingly to perform a gentle push into the nozzle
  after fsensor detects the filament + update its unit tests.
- slight cleanup of LoadFilament + fix its unit tests
- add FeedingToNozzle progress code, as it might be interesting
  to inform the printer about this task in the future
- revert non-clean changes from RetractFromFinda - it should not disengage the idler
- revert incorrect + fix ToolChange
- clean-up UnloadFilament

src/config/config.h

@@ -77,6 +77,7 @@ static constexpr U_mm minimumBowdenLength = 341.0_mm; /// ~341.0_mm /// Minimum
 static constexpr U_mm maximumBowdenLength = 792.0_mm; /// ~792.0_mm /// Maximum bowden length. @TODO Should be stored in EEPROM.
 static constexpr U_mm feedToFinda = cuttingEdgeToFindaMidpoint + filamentMinLoadedToMMU;
 static constexpr U_mm cutLength = 8.0_mm;
+static constexpr U_mm fsensorToNozzle = 20.0_mm; /// ~20mm from MK4's filament sensor through extruder gears into nozzle
 
 /// Begin: Pulley axis configuration
 static constexpr AxisConfig pulley = {
@@ -96,6 +97,7 @@ static constexpr PulleyLimits pulleyLimits = {
     .accel = 800.0_mm_s2,
 };
 static constexpr U_mm_s pulleyFeedrate = 40._mm_s;
+static constexpr U_mm_s pulleySlowFeedrate = 1._mm_s;
 /// End: Pulley axis configuration
 
 /// Begin: Selector configuration

src/logic/feed_to_bondtech.cpp

@@ -24,19 +24,21 @@ bool FeedToBondtech::Step() {
         if (mi::idler.Engaged()) {
             dbg_logic_P(PSTR("Feed to Bondtech --> Idler engaged"));
             dbg_logic_sprintf_P(PSTR("Pulley start steps %u"), mm::motion.CurPosition(mm::Pulley));
-            state = PushingFilament;
+            state = PushingFilamentToFSensor;
             mm::motion.InitAxis(mm::Pulley);
             mm::motion.PlanMove<mm::Pulley>(config::defaultBowdenLength, config::pulleyFeedrate); //@@TODO constants - there was some strange acceleration sequence in the original FW,
             // we can probably hand over some array of constants for hand-tuned acceleration + leverage some smoothing in the stepper as well
         }
         return false;
-    case PushingFilament:
+    case PushingFilamentToFSensor:
         //dbg_logic_P(PSTR("Feed to Bondtech --> Pushing"));
         if (mfs::fsensor.Pressed()) {
             mm::motion.AbortPlannedMoves(); // stop pushing filament
-            mi::idler.Disengage();
+            mg::globals.SetFilamentLoaded(mg::FilamentLoadState::InFSensor);
-            mg::globals.SetFilamentLoaded(mg::FilamentLoadState::InNozzle);
+            // plan a slow move to help push filament into the nozzle
-            state = DisengagingIdler;
+            //@@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
@@ -44,6 +46,13 @@ bool FeedToBondtech::Step() {
             state = Failed;
         }
         return false;
+    case PushingFilamentIntoNozzle:
+        if (mm::motion.QueueEmpty()) {
+            mg::globals.SetFilamentLoaded(mg::FilamentLoadState::InNozzle);
+            mi::idler.Disengage();
+            state = DisengagingIdler;
+        }
+        return false;
     case DisengagingIdler:
         if (!mi::idler.Engaged()) {
             dbg_logic_P(PSTR("Feed to Bondtech --> Idler disengaged"));

src/logic/feed_to_bondtech.h

@@ -6,14 +6,15 @@ namespace logic {
 /// @brief Feed filament to Bondtech gears of the printer
 ///
 /// Continuously feed filament until the printer detects the filament in its filament sensor.
+/// Then it feeds a bit more very gently to push the filament into the nozzle
 /// Disengages the Idler after finishing the feed.
 /// Disables the Pulley axis after disengaging the idler.
 struct FeedToBondtech {
     /// internal states of the state machine
     enum {
         EngagingIdler,
-        PushingFilament,
+        PushingFilamentToFSensor,
-        UnloadBackToPTFE,
+        PushingFilamentIntoNozzle,
         DisengagingIdler,
         OK,
         Failed

src/logic/load_filament.cpp

@@ -48,12 +48,12 @@ bool LoadFilament::StepInner() {
             if (retract.State() == RetractFromFinda::Failed) {
                 state = ProgressCode::ERRDisengagingIdler;
                 error = ErrorCode::FINDA_DIDNT_SWITCH_OFF;
-                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 {
                 state = ProgressCode::DisengagingIdler;
             }
+            mi::idler.Disengage(); // disengage in both cases
         }
         break;
     case ProgressCode::DisengagingIdler:

src/logic/progress_codes.h

@@ -14,24 +14,25 @@ enum class ProgressCode : uint_fast8_t {
     UnloadingToPulley, //P4
     FeedingToFinda, // P5
     FeedingToBondtech, // P6
-    AvoidingGrind, // P7
+    FeedingToNozzle, // P7
-    FinishingMoves, // P8
+    AvoidingGrind, // P8
+    FinishingMoves, // P9
 
-    ERRDisengagingIdler, // P9
+    ERRDisengagingIdler, // P10
-    ERREngagingIdler, // P10
+    ERREngagingIdler, // P11
-    ERRWaitingForUser, // P11
+    ERRWaitingForUser, // P12
-    ERRInternal, // P12
+    ERRInternal, // P13
-    ERRHelpingFilament, // P13
+    ERRHelpingFilament, // P14
-    ERRTMCFailed, // P14
+    ERRTMCFailed, // P15
 
-    UnloadingFilament, // P15
+    UnloadingFilament, // P16
-    LoadingFilament, // P16
+    LoadingFilament, // P17
-    SelectingFilamentSlot, // P17
+    SelectingFilamentSlot, // P18
-    PreparingBlade, // P18
+    PreparingBlade, // P19
-    PushingFilament, // P19
+    PushingFilament, // P20
-    PerformingCut, // P20
+    PerformingCut, // P21
-    ReturningSelector, // P21
+    ReturningSelector, // P22
-    ParkingSelector, // P22
+    ParkingSelector, // P23
-    EjectingFilament, // P23
+    EjectingFilament, // P24
-    RetractingFromFinda, // P24
+    RetractingFromFinda, // P25
 };

src/logic/retract_from_finda.cpp

@@ -30,18 +30,14 @@ bool RetractFromFinda::Step() {
             if (!mf::finda.Pressed()) { // FINDA switched off correctly while the move was performed
                 state = OK;
                 mg::globals.SetFilamentLoaded(mg::FilamentLoadState::AtPulley);
-                mi::idler.Disengage();
+                dbg_logic_sprintf_P(PSTR("Pulley end steps %u"), mm::motion.CurPosition(mm::Pulley));
+                ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
             } else { // FINDA didn't switch off
                 state = Failed;
                 ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
                 ml::leds.SetMode(mg::globals.ActiveSlot(), ml::red, ml::blink0);
             }
         }
-        if (!mi::idler.Engaged()) {
-            dbg_logic_P(PSTR("Retract from FINDA --> Ider disengaged"));
-            dbg_logic_sprintf_P(PSTR("Pulley end steps %u"), mm::motion.CurPosition(mm::Pulley));
-            ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::off);
-        }
         return false;
     case OK:
         dbg_logic_P(PSTR("Retract from FINDA OK"));

src/logic/retract_from_finda.h

@@ -5,7 +5,11 @@ namespace logic {
 
 /// @brief Retract filament from FINDA to PTFE
 ///
-/// Continuously pull filament by a fixed length (originally 600 steps) + verify FINDA is switched OFF
+/// Continuously pulls filament by a fixed length (originally 600 steps) + verifies FINDA is switched OFF while performing the move
+/// Steps:
+/// - engages idler (or makes sure the idler is engaged)
+/// - pulls filament
+/// - leaves idler engaged for chaining operations
 struct RetractFromFinda {
     /// internal states of the state machine
     enum {

src/logic/tool_change.cpp

@@ -19,8 +19,8 @@ void ToolChange::Reset(uint8_t param) {
         return;
     }
 
-    if (param == mg::globals.ActiveSlot() && (mg::globals.FilamentLoaded() >= mg::FilamentLoadState::InSelector)) {
+    if (param == mg::globals.ActiveSlot() && (mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle)) {
-        // we are already at the correct slot and the filament is loaded - nothing to do
+        // we are already at the correct slot and the filament is loaded in the nozzle - nothing to do
         dbg_logic_P(PSTR("we are already at the correct slot and the filament is loaded - nothing to do\n"));
         return;
     }
@@ -78,7 +78,6 @@ bool ToolChange::StepInner() {
                 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(mg::FilamentLoadState::InFSensor);
                 state = ProgressCode::OK;
                 error = ErrorCode::OK;
             }

src/logic/unload_filament.cpp

@@ -45,16 +45,16 @@ bool UnloadFilament::StepInner() {
             if (retract.State() == RetractFromFinda::Failed) {
                 state = ProgressCode::ERRDisengagingIdler;
                 error = ErrorCode::FINDA_DIDNT_SWITCH_OFF;
-                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 {
-                state = ProgressCode::FinishingMoves;
+                state = ProgressCode::DisengagingIdler;
             }
+            mi::idler.Disengage();
         }
         return false;
-    case ProgressCode::FinishingMoves:
+    case ProgressCode::DisengagingIdler:
-        if (mm::motion.QueueEmpty()) {
+        if (!mi::idler.Engaged()) {
             state = ProgressCode::OK;
             error = ErrorCode::OK;
             mm::motion.Disable(mm::Pulley);

tests/unit/logic/cut_filament/CMakeLists.txt

@@ -4,6 +4,7 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/cut_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp

tests/unit/logic/eject_filament/CMakeLists.txt

@@ -4,6 +4,7 @@ add_executable(
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/eject_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp

tests/unit/logic/failing_tmc/CMakeLists.txt

@@ -3,6 +3,7 @@ add_executable(
   failing_tmc_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp

tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp

@@ -51,7 +51,7 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
     CHECK(mm::axes[mm::Pulley].enabled);
 
     // idler engaged, selector in position, we'll start pushing filament
-    REQUIRE(fb.State() == FeedToBondtech::PushingFilament);
+    REQUIRE(fb.State() == FeedToBondtech::PushingFilamentToFSensor);
     // at least at the beginning the LED should shine green (it should be blinking, but this mode has been already verified in the LED's unit test)
     REQUIRE(ml::leds.LedOn(mg::globals.ActiveSlot(), ml::green));
 
@@ -61,11 +61,18 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
         if( step == 1000 ){
             mfs::fsensor.ProcessMessage(true);
         }
-        return fb.State() == FeedToBondtech::PushingFilament; },
+        return fb.State() == FeedToBondtech::PushingFilamentToFSensor; },
         1500));
 
     REQUIRE(mfs::fsensor.Pressed());
 
+    // pushing filament from fsensor into the nozzle
+    REQUIRE(fb.State() == FeedToBondtech::PushingFilamentIntoNozzle);
+    REQUIRE(WhileCondition(
+        fb,
+        [&](int) { return fb.State() == FeedToBondtech::PushingFilamentIntoNozzle; },
+        5000));
+
     // disengaging idler
     REQUIRE(fb.State() == FeedToBondtech::DisengagingIdler);
     REQUIRE(WhileCondition(

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -39,11 +39,7 @@ void LoadFilamentCommonSetup(uint8_t slot, logic::LoadFilament &lf) {
     // no change in selector's position
     // FINDA off
     // green LED should blink, red off
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::EngagingIdler));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
-
-    // Stage 1 - engaging idler
-    REQUIRE(WhileTopState(lf, ProgressCode::EngagingIdler, idlerEngageDisengageMaxSteps));
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
 }
 
 void LoadFilamentSuccessful(uint8_t slot, logic::LoadFilament &lf) {
@@ -69,7 +65,7 @@ void LoadFilamentSuccessful(uint8_t slot, logic::LoadFilament &lf) {
         }
         return lf.TopLevelState() == ProgressCode::RetractingFromFinda; },
         5000));
-    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler));
 
     // Stage 4 - disengaging idler
     REQUIRE(WhileTopState(lf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));

tests/unit/logic/unload_filament/CMakeLists.txt

@@ -3,6 +3,7 @@ add_executable(
   unload_filament_tests
   ${CMAKE_SOURCE_DIR}/src/logic/command_base.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/feed_to_finda.cpp
+  ${CMAKE_SOURCE_DIR}/src/logic/retract_from_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_filament.cpp
   ${CMAKE_SOURCE_DIR}/src/logic/unload_to_finda.cpp
   ${CMAKE_SOURCE_DIR}/src/modules/buttons.cpp

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -66,36 +66,19 @@ void RegularUnloadFromSlot04(uint8_t slot, logic::UnloadFilament &uf) {
     // no change in selector's position
     // FINDA triggered off
     // green LED should be off
-    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler));
+    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, slot, slot, false, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
 
-    // Stage 2 - idler was engaged, disengage it
+    // Stage 2 - retracting from FINDA
+    REQUIRE(WhileTopState(uf, ProgressCode::RetractingFromFinda, idlerEngageDisengageMaxSteps));
+
+    // Stage 3 - idler was engaged, disengage it
     REQUIRE(WhileTopState(uf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps));
 
-    // we still think we have filament loaded at this stage
-    // idler should have been disengaged
-    // no change in selector's position
-    // FINDA still triggered off
-    // green LED should be off
-    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::AvoidingGrind));
-
-    // Stage 3 - avoiding grind (whatever is that @@TODO)
-    REQUIRE(WhileTopState(uf, ProgressCode::AvoidingGrind, 5000));
-
-    // we still think we have filament loaded at this stage
-    // idler should have been disengaged
-    // no change in selector's position
-    // FINDA still triggered off
-    // green LED should be off
-    REQUIRE(VerifyState(uf, mg::FilamentLoadState::InSelector, mi::Idler::IdleSlotIndex(), slot, false, true, ml::off, ml::off, ErrorCode::RUNNING, ProgressCode::FinishingMoves));
-
-    // Stage 4 - finishing moves and setting global state correctly
-    REQUIRE(WhileTopState(uf, ProgressCode::FinishingMoves, 5000));
-
     // filament unloaded
     // idler should have been disengaged
     // no change in selector's position
     // FINDA still triggered off
-    // green LED should be OFF
+    // green LED should be off
     REQUIRE(VerifyState(uf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
 
     // Stage 5 - repeated calls to TopLevelState should return "OK"