Make sure the selector never moves if FINDA is pressed

It looks we have some kind of leak when filament sensor state is not completely coherent with FINDA state.
This is yet to be discovered and fixed with some unit tests.

src/modules/selector.cpp

@@ -65,6 +65,12 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
         return OperationResult::Accepted;
     }
 
+    if (mf::finda.Pressed()) {
+        // @@TODO not sure why (if) this happens, but anyway - we must not move the selector if FINDA is pressed
+        // That includes the CutFilament operation as well
+        return OperationResult::Refused;
+    }
+
     // coordinates invalid, first home, then engage
     if (!homingValid && mg::globals.FilamentLoaded() < mg::FilamentLoadState::InSelector) {
         PlanHome();
@@ -96,7 +102,7 @@ bool Selector::Step() {
         PerformHomeBack();
         return false;
     case Ready:
-        if (!homingValid && mg::globals.FilamentLoaded() < mg::InSelector) {
+        if (!homingValid && mg::globals.FilamentLoaded() < mg::InSelector && (!mf::finda.Pressed())) {
             PlanHome();
             return false;
         }

tests/unit/logic/cut_filament/test_cut_filament.cpp

@@ -24,7 +24,7 @@ using Catch::Matchers::Equals;
 void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) {
 
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off));
 

tests/unit/logic/eject_filament/test_eject_filament.cpp

@@ -26,7 +26,7 @@ TEST_CASE("eject_filament::eject0", "[eject_filament][.]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     EjectFilament ef;
     // restart the automaton

tests/unit/logic/failing_tmc/test_failing_tmc.cpp

@@ -40,7 +40,7 @@ void FailingMovableUnload(hal::tmc2130::ErrorFlags ef, ErrorCode ec, config::Axi
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::InNozzle);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::InNozzle));
 
     // set FINDA ON + debounce
     SetFINDAStateAndDebounce(true);
@@ -90,7 +90,7 @@ void FailingMovableLoad(hal::tmc2130::ErrorFlags ef, ErrorCode ec, config::Axis
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(5, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(5, mg::FilamentLoadState::AtPulley));
 
     // set FINDA OFF + debounce
     SetFINDAStateAndDebounce(false);

tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp

@@ -25,7 +25,7 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     FeedToBondtech fb;
     main_loop();

tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp

@@ -25,7 +25,7 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     FeedToFinda ff;
     main_loop();
@@ -93,7 +93,7 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
     using namespace logic;
 
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     FeedToFinda ff;
     main_loop();

tests/unit/logic/homing/test_homing.cpp

@@ -29,7 +29,7 @@ bool SuccessfulHome(uint8_t slot) {
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley));
 
     // set FINDA OFF + debounce
     SetFINDAStateAndDebounce(false);
@@ -64,7 +64,7 @@ bool SelectorFailedRetry() {
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // set FINDA OFF + debounce
     SetFINDAStateAndDebounce(false);
@@ -95,3 +95,58 @@ bool SelectorFailedRetry() {
 TEST_CASE("homing::selector_failed_retry", "[homing]") {
     REQUIRE(SelectorFailedRetry());
 }
+
+bool RefusedMove(uint8_t slot) {
+    // prepare startup conditions
+    ForceReinitAllAutomata();
+
+    // change the startup to what we need here
+    HomeIdlerAndSelector();
+
+    SetFINDAStateAndDebounce(true);
+    mg::globals.SetFilamentLoaded(slot, mg::FilamentLoadState::InSelector);
+
+    // move selector to the right spot - should not be possible
+    REQUIRE(ms::selector.MoveToSlot(slot) == ms::Selector::OperationResult::Refused);
+    return true;
+}
+
+bool RefusedHome(uint8_t slot) {
+    // prepare startup conditions
+    ForceReinitAllAutomata();
+
+    // change the startup to what we need here
+    HomeIdlerAndSelector();
+
+    SetFINDAStateAndDebounce(true);
+    mg::globals.SetFilamentLoaded(slot, mg::FilamentLoadState::InSelector);
+
+    ms::selector.InvalidateHoming();
+
+    // selector should not start homing, because something is in the FINDA
+    for (uint8_t i = 0; i < 100; ++i) {
+        main_loop();
+        REQUIRE_FALSE(ms::selector.HomingValid());
+        REQUIRE(ms::selector.State() == ms::Selector::Ready);
+    }
+    // unpress FINDA
+    SetFINDAStateAndDebounce(false);
+    mg::globals.SetFilamentLoaded(slot, mg::FilamentLoadState::AtPulley);
+
+    // selector should start the homing sequence
+    main_loop();
+    REQUIRE(ms::selector.State() == ms::Selector::HomeForward);
+    return true;
+}
+
+TEST_CASE("homing::refused_move", "[homing]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        REQUIRE(RefusedMove(slot));
+    }
+}
+
+TEST_CASE("homing::refused_home", "[homing]") {
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        REQUIRE(RefusedHome(slot));
+    }
+}

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -28,7 +28,7 @@ void LoadFilamentCommonSetup(uint8_t slot, logic::LoadFilament &lf, bool feedLim
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley));
 
     // verify startup conditions
     REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
@@ -375,8 +375,8 @@ TEST_CASE("load_filament::avoid_load_filament_finda", "[load_filament]") {
         for (uint8_t activeSlot = 0; activeSlot < config::toolCount; ++activeSlot) {
             logic::LoadFilament lf;
             ForceReinitAllAutomata();
-            SetFINDAStateAndDebounce(true);
-            EnsureActiveSlotIndex(activeSlot, fls);
+            REQUIRE(EnsureActiveSlotIndex(activeSlot, fls));
+            SetFINDAStateAndDebounce(true); // beware - selector will refuse to move if FINDA is pressed - must set active slot first and then FINDA
             REQUIRE(VerifyState(lf, fls, mi::Idler::IdleSlotIndex(), activeSlot, true, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));
             bool accepted = lf.Reset(slot);
             if (activeSlot != slot) {

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -84,16 +84,18 @@ void HomeIdlerAndSelector() {
     SimulateIdlerAndSelectorHoming(nc);
 }
 
-void EnsureActiveSlotIndex(uint8_t slot, mg::FilamentLoadState loadState) {
+bool EnsureActiveSlotIndex(uint8_t slot, mg::FilamentLoadState loadState) {
     HomeIdlerAndSelector();
 
     // move selector to the right spot
-    ms::selector.MoveToSlot(slot);
+    if (ms::selector.MoveToSlot(slot) == ms::Selector::OperationResult::Refused)
+        return false;
     while (ms::selector.Slot() != slot)
         main_loop();
 
     // mg::globals.SetActiveSlot(slot);
     mg::globals.SetFilamentLoaded(slot, loadState);
+    return true;
 }
 
 void SetFINDAStateAndDebounce(bool press) {

tests/unit/logic/stubs/main_loop_stub.h

@@ -22,7 +22,7 @@ bool WhileTopState(SM &sm, ProgressCode state, uint32_t maxLoops = 5000) {
         sm, [&](uint32_t) { return sm.TopLevelState() == state; }, maxLoops);
 }
 
-void EnsureActiveSlotIndex(uint8_t slot, modules::globals::FilamentLoadState loadState);
+bool EnsureActiveSlotIndex(uint8_t slot, modules::globals::FilamentLoadState loadState);
 
 void SetFINDAStateAndDebounce(bool press);
 bool SimulateUnloadToFINDA(uint32_t step, uint32_t fsOff, uint32_t findaOff);
@@ -37,3 +37,5 @@ void ClearButtons(logic::CommandBase &cb);
 // ... could be computed in the future from the pre-set number of microsteps and real positions
 static constexpr uint32_t idlerEngageDisengageMaxSteps = 40000UL;
 static constexpr uint32_t selectorMoveMaxSteps = 40000UL;
+
+void HomeIdlerAndSelector();

tests/unit/logic/tool_change/test_tool_change.cpp

@@ -65,9 +65,9 @@ void CheckFinishedCorrectly(logic::ToolChange &tc, uint8_t toSlot) {
 void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
+    REQUIRE(EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle));
     SetFINDAStateAndDebounce(true);
     mfs::fsensor.ProcessMessage(true);
-    EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
 
     // restart the automaton
     tc.Reset(toSlot);
@@ -96,10 +96,10 @@ void ToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
 void NoToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
+    REQUIRE(EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle));
     // the filament is LOADED
-    SetFINDAStateAndDebounce(true);
     mfs::fsensor.ProcessMessage(true);
-    EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
+    SetFINDAStateAndDebounce(true);
 
     REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::InNozzle, mi::Idler::IdleSlotIndex(), toSlot, true, false, ml::off, ml::off));
 
@@ -114,7 +114,7 @@ void NoToolChange(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
 void JustLoadFilament(logic::ToolChange &tc, uint8_t slot) {
     ForceReinitAllAutomata();
 
-    EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley));
 
     // verify filament NOT loaded
     REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), slot, false, false, ml::off, ml::off));
@@ -175,9 +175,9 @@ TEST_CASE("tool_change::same_slot_just_unloaded_filament", "[tool_change]") {
 void ToolChangeFailLoadToFinda(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSlot) {
     ForceReinitAllAutomata();
 
+    REQUIRE(EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle));
     SetFINDAStateAndDebounce(true);
     mfs::fsensor.ProcessMessage(true);
-    EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
 
     // restart the automaton
     tc.Reset(toSlot);
@@ -330,9 +330,9 @@ void ToolChangeFailFSensor(logic::ToolChange &tc, uint8_t fromSlot, uint8_t toSl
     using namespace std::placeholders;
     ForceReinitAllAutomata();
 
+    REQUIRE(EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle));
     SetFINDAStateAndDebounce(true);
     mfs::fsensor.ProcessMessage(true);
-    EnsureActiveSlotIndex(fromSlot, mg::FilamentLoadState::InNozzle);
 
     // restart the automaton
     tc.Reset(toSlot);

tests/unit/logic/unload_filament/test_unload_filament.cpp

@@ -28,7 +28,7 @@ void RegularUnloadFromSlot04Init(uint8_t slot, logic::UnloadFilament &uf) {
     ForceReinitAllAutomata();
 
     // change the startup to what we need here
-    EnsureActiveSlotIndex(slot, mg::FilamentLoadState::InNozzle);
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::InNozzle));
 
     // set FINDA ON + debounce
     SetFINDAStateAndDebounce(true);
@@ -108,7 +108,7 @@ void FindaDidntTriggerCommonSetup(uint8_t slot, logic::UnloadFilament &uf) {
 
     // change the startup to what we need here
     // move selector to the right spot
-    EnsureActiveSlotIndex(slot, mg::FilamentLoadState::InNozzle);
+    REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::InNozzle));
 
     // set FINDA ON + debounce
     SetFINDAStateAndDebounce(true);
@@ -279,7 +279,7 @@ TEST_CASE("unload_filament::not_loaded", "[unload_filament]") {
 
     // change the startup to what we need here
     // move selector to the right spot
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // verify startup conditions
     REQUIRE(VerifyState(uf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off, ErrorCode::OK, ProgressCode::OK));

tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp

@@ -26,7 +26,7 @@ namespace ha = hal::adc;
 
 TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
@@ -64,7 +64,7 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
 
 TEST_CASE("unload_to_finda::no_sense_FINDA_upon_start", "[unload_to_finda]") {
     ForceReinitAllAutomata(); // that implies FINDA OFF which should really not happen for an unload call
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     logic::UnloadToFinda ff;
 
@@ -78,7 +78,7 @@ TEST_CASE("unload_to_finda::no_sense_FINDA_upon_start", "[unload_to_finda]") {
 
 TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]") {
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
@@ -119,7 +119,7 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]")
 
 TEST_CASE("unload_to_finda::unload_without_FSensor_trigger", "[unload_to_finda]") {
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);
@@ -160,7 +160,7 @@ TEST_CASE("unload_to_finda::unload_without_FSensor_trigger", "[unload_to_finda]"
 
 TEST_CASE("unload_to_finda::unload_repeated", "[unload_to_finda]") {
     ForceReinitAllAutomata();
-    EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley);
+    REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
 
     // we need finda ON
     SetFINDAStateAndDebounce(true);