Reject LoadFilament if filament state > AtPulley

If the MMU receives a command LoadFilament with a slot number SL we reject
the command to avoid moving the selector (effectively cutting the piece of filament present in FINDA).

That includes the scenario when the selector is standing at the very same slot SL, because the filament could be held by the printer (i.e. loaded in the nozzle).

There is one special case though - same slot AND filament load state == InSelector (it MUST NOT be anywhere farther)

src/application.cpp

@@ -128,8 +128,8 @@ void Application::PlanCommand(const modules::protocol::RequestMsg &rq) {
             break;
         }
         currentCommandRq = rq; // save the Current Command Request for indentification of responses
-        currentCommand->Reset(rq.value);
-        ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Accepted);
+        bool accepted = currentCommand->Reset(rq.value);
+        ReportCommandAccepted(rq, accepted ? mp::ResponseMsgParamCodes::Accepted : mp::ResponseMsgParamCodes::Rejected);
     } else {
         ReportCommandAccepted(rq, mp::ResponseMsgParamCodes::Rejected);
     }

src/logic/command_base.h

@@ -38,7 +38,8 @@ public:
 
     /// resets the automaton
     /// @param param numerical parameter that comes with some commands (e.g. T1 for tool change 1)
-    virtual void Reset(uint8_t param) = 0;
+    /// @returns true if the command was accepted and started (which may not be possible e.g. due to filament position)
+    virtual bool Reset(uint8_t param) = 0;
 
     /// Steps the state machine. This is the preferred way of stepping the machine
     /// as it handles the global HW error states uniformly (so that the derived classes do not have to deal

src/logic/cut_filament.cpp

@@ -14,9 +14,9 @@ namespace logic {
 
 CutFilament cutFilament;
 
-void CutFilament::Reset(uint8_t param) {
+bool CutFilament::Reset(uint8_t param) {
     if (!CheckToolIndex(param)) {
-        return;
+        return false;
     }
 
     error = ErrorCode::RUNNING;
@@ -28,6 +28,7 @@ void CutFilament::Reset(uint8_t param) {
     } else {
         SelectFilamentSlot();
     }
+    return true;
 }
 
 void CutFilament::SelectFilamentSlot() {

src/logic/cut_filament.h

@@ -15,7 +15,7 @@ public:
 
     /// Restart the automaton
     /// @param param index of filament slot to perform cut onto
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

src/logic/eject_filament.cpp

@@ -15,9 +15,9 @@ namespace logic {
 
 EjectFilament ejectFilament;
 
-void EjectFilament::Reset(uint8_t param) {
+bool EjectFilament::Reset(uint8_t param) {
     if (!CheckToolIndex(param)) {
-        return;
+        return false;
     }
 
     error = ErrorCode::RUNNING;
@@ -32,6 +32,7 @@ void EjectFilament::Reset(uint8_t param) {
     } else {
         MoveSelectorAside();
     }
+    return true;
 }
 
 void EjectFilament::MoveSelectorAside() {

src/logic/eject_filament.h

@@ -27,7 +27,7 @@ public:
 
     /// Restart the automaton
     /// @param param index of filament slot to eject
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

src/logic/home.cpp

@@ -12,10 +12,11 @@ namespace logic {
 
 Home home;
 
-void Home::Reset(uint8_t /*param*/) {
+bool Home::Reset(uint8_t /*param*/) {
     error = ErrorCode::RUNNING;
     state = ProgressCode::Homing;
     InvalidateHomingAndFilamentState();
+    return true;
 }
 
 bool Home::StepInner() {

src/logic/home.h

@@ -25,7 +25,7 @@ public:
 
     /// Restart the automaton
     /// @param param unused
-    void Reset(uint8_t /*param*/) override;
+    bool Reset(uint8_t /*param*/) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

src/logic/load_filament.cpp

@@ -15,14 +15,22 @@ namespace logic {
 
 LoadFilament loadFilament;
 
-void LoadFilament::Reset(uint8_t param) {
+bool LoadFilament::Reset(uint8_t param) {
     if (!CheckToolIndex(param)) {
-        return;
+        return false;
+    }
+    if (mg::globals.FilamentLoaded() > mg::FilamentLoadState::InSelector) {
+        return false; // avoid starting loadfilament if there is already some filament in the selector in some OTHER slot
+    }
+    // there is one special case though - same slot AND filament load state == InSelector (it MUST NOT be anywhere farther)
+    if (mg::globals.FilamentLoaded() > mg::FilamentLoadState::AtPulley && mg::globals.ActiveSlot() != param) {
+        return false;
     }
     dbg_logic_P(PSTR("Load Filament"));
     mg::globals.SetFilamentLoaded(param, mg::FilamentLoadState::AtPulley); // still at pulley, haven't moved yet
     verifyLoadedFilament = 1;
     Reset2(false);
+    return true;
 }
 
 void LoadFilament::ResetLimited(uint8_t param) {

src/logic/load_filament.h

@@ -16,7 +16,7 @@ public:
 
     /// Restart the automaton - performs unlimited rotation of the Pulley
     /// @param param index of filament slot to load
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// Restart the automaton for a limited rotation of the Pulley
     /// @param param index of filament slot to load

src/logic/move_selector.cpp

@@ -8,14 +8,16 @@ namespace logic {
 
 MoveSelector moveSelector;
 
-void MoveSelector::Reset(uint8_t param) {
+bool MoveSelector::Reset(uint8_t param) {
     state = ProgressCode::MovingSelector;
 
     if (ms::selector.MoveToSlot(param) != ms::Selector::OperationResult::Refused) {
         // operation accepted
         error = ErrorCode::RUNNING;
+        return true;
     } else {
         error = ErrorCode::HOMING_SELECTOR_FAILED; // @@TODO
+        return false;
     }
 }
 

src/logic/move_selector.h

@@ -20,7 +20,7 @@ public:
 
     /// Restart the automaton
     /// @param param target selector slot
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

src/logic/no_command.h

@@ -12,7 +12,7 @@ public:
         : CommandBase() {}
 
     /// Restart the automaton
-    void Reset(uint8_t /*param*/) override {}
+    bool Reset(uint8_t /*param*/) override { return true; }
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override { return true; }

src/logic/set_mode.cpp

@@ -7,11 +7,12 @@ namespace logic {
 
 SetMode setMode;
 
-void SetMode::Reset(uint8_t param) {
+bool SetMode::Reset(uint8_t param) {
     mg::globals.SetMotorsMode(param != 0); // remember the last mode set
     // distribute the mode to all motors immediately
     mm::motion.SetMode((param == 0) ? mm::Normal : mm::Stealth);
     FinishedOK();
+    return true;
 }
 
 } // namespace logic

src/logic/set_mode.h

@@ -17,7 +17,7 @@ public:
         : CommandBase() {}
 
     /// Restart the automaton
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     /// Since we perform the TMC2130 mode change in the Reset directly, the return is always true here (command finished ok)

src/logic/tool_change.cpp

@@ -17,15 +17,15 @@ namespace logic {
 
 ToolChange toolChange;
 
-void ToolChange::Reset(uint8_t param) {
+bool ToolChange::Reset(uint8_t param) {
     if (!CheckToolIndex(param)) {
-        return;
+        return false;
     }
 
     if (param == mg::globals.ActiveSlot() && (mg::globals.FilamentLoaded() == mg::FilamentLoadState::InNozzle)) {
         // 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;
+        return true;
     }
 
     // we are either already at the correct slot, just the filament is not loaded - load the filament directly
@@ -43,6 +43,7 @@ void ToolChange::Reset(uint8_t param) {
         mg::globals.SetFilamentLoaded(plannedSlot, mg::FilamentLoadState::InSelector);
         feed.Reset(true, false);
     }
+    return true;
 }
 
 void logic::ToolChange::GoToFeedingToBondtech() {

src/logic/tool_change.h

@@ -16,7 +16,7 @@ public:
 
     /// Restart the automaton
     /// @param param index of filament slot to change to - i.e. to load
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

src/logic/unload_filament.cpp

@@ -16,12 +16,12 @@ namespace logic {
 
 UnloadFilament unloadFilament;
 
-void UnloadFilament::Reset(uint8_t /*param*/) {
+bool UnloadFilament::Reset(uint8_t /*param*/) {
 
     if (!mf::finda.Pressed() && mg::globals.FilamentLoaded() < mg::FilamentLoadState::InSelector) {
         // it looks like we have nothing in the PTFE tube, at least FINDA doesn't sense anything
         // so the filament has been probably already unloaded - terminate with OK or report an error?
-        return;
+        return true;
     }
 
     // unloads filament from extruder - filament is above Bondtech gears
@@ -30,6 +30,7 @@ void UnloadFilament::Reset(uint8_t /*param*/) {
     error = ErrorCode::RUNNING;
     unl.Reset(maxRetries);
     ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::off, ml::off);
+    return true;
 }
 
 void UnloadFilament::UnloadFinishedCorrectly() {

src/logic/unload_filament.h

@@ -16,7 +16,7 @@ public:
 
     /// Restart the automaton
     /// @param param is not used, always unloads from the active slot
-    void Reset(uint8_t param) override;
+    bool Reset(uint8_t param) override;
 
     /// @returns true if the state machine finished its job, false otherwise
     bool StepInner() override;

tests/unit/logic/load_filament/test_load_filament.cpp

@@ -351,3 +351,41 @@ TEST_CASE("load_filament::unlimited_load_manual_stop", "[load_filament]") {
         LoadFilamentStopped(slot, lf);
     }
 }
+
+void LoadFilamentAlreadyPresentFilament(uint8_t slot, logic::LoadFilament &lf) {
+    //one of the first steps of the state machine should pick up the fact that FINDA is on and transfer into the retracting phase
+    REQUIRE(WhileTopState(lf, ProgressCode::FeedingToFinda, 5000));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
+    REQUIRE(WhileCondition(lf, std::bind(SimulateRetractFromFINDA, _1, 100), 5000));
+    REQUIRE(WhileTopState(lf, ProgressCode::RetractingFromFinda, 5000));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::AtPulley, slot, slot, false, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::FeedingToFinda));
+    // make FINDA switch on again
+    REQUIRE(WhileCondition(lf, std::bind(SimulateFeedToFINDA, _1, 100), 5000));
+    REQUIRE(WhileTopState(lf, ProgressCode::FeedingToFinda, 5000));
+    REQUIRE(VerifyState(lf, mg::FilamentLoadState::InSelector, slot, slot, true, true, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda));
+    // make FINDA switch off again
+    REQUIRE(WhileCondition(lf, std::bind(SimulateRetractFromFINDA, _1, 100), 5000));
+    REQUIRE(WhileTopState(lf, ProgressCode::RetractingFromFinda, 5000));
+}
+
+TEST_CASE("load_filament::avoid_load_filament_finda", "[load_filament]") {
+    auto fls = GENERATE(mg::FilamentLoadState::InSelector, mg::FilamentLoadState::InFSensor, mg::FilamentLoadState::InNozzle);
+
+    for (uint8_t slot = 0; slot < config::toolCount; ++slot) {
+        for (uint8_t activeSlot = 0; activeSlot < config::toolCount; ++activeSlot) {
+            logic::LoadFilament lf;
+            ForceReinitAllAutomata();
+            SetFINDAStateAndDebounce(true);
+            EnsureActiveSlotIndex(activeSlot, fls);
+            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) {
+                REQUIRE_FALSE(accepted);
+            } else if (activeSlot == slot && fls <= mg::FilamentLoadState::InSelector) {
+                LoadFilamentAlreadyPresentFilament(slot, lf);
+            } else {
+                REQUIRE_FALSE(accepted);
+            }
+        }
+    }
+}