From d75119c8d62a39c14bb1314b0153c20c6bba7f23 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 24 Feb 2023 16:16:39 +0100
Subject: [PATCH] Suspend all moves of Idler and Selector when entering an
 Error screen

---
 src/logic/command_base.cpp   | 14 ++++++++++++++
 src/logic/command_base.h     |  9 ++++++++-
 src/logic/eject_filament.cpp |  1 +
 src/logic/load_filament.cpp  |  1 +
 src/modules/idler.cpp        |  2 ++
 src/modules/movable_base.h   |  8 +++++++-
 src/modules/pulley.cpp       |  2 ++
 src/modules/selector.cpp     |  2 ++
 8 files changed, 37 insertions(+), 2 deletions(-)

diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp
index 6848bfa..bbed5f0 100644
--- a/src/logic/command_base.cpp
+++ b/src/logic/command_base.cpp
@@ -153,6 +153,7 @@ void CommandBase::Panic(ErrorCode ec) {
 }
 
 void CommandBase::InvalidateHoming() {
+    ResumeIdlerSelector();
     mi::idler.InvalidateHoming();
     ms::selector.InvalidateHoming();
 }
@@ -177,6 +178,18 @@ void CommandBase::InvalidateHomingAndFilamentState() {
     }
 }
 
+void CommandBase::HoldIdlerSelector() {
+    mm::motion.AbortPlannedMoves(mm::Idler);
+    mi::idler.HoldOn();
+    mm::motion.AbortPlannedMoves(mm::Selector);
+    ms::selector.HoldOn();
+}
+
+void CommandBase::ResumeIdlerSelector() {
+    mi::idler.Resume();
+    ms::selector.Resume();
+}
+
 bool CommandBase::CheckToolIndex(uint8_t index) {
     if (index >= config::toolCount) {
         error = ErrorCode::INVALID_TOOL;
@@ -195,6 +208,7 @@ void CommandBase::ErrDisengagingIdler() {
         mg::globals.IncDriveErrors();
         mpu::pulley.Disable();
         mui::userInput.Clear(); // remove all buffered events if any just before we wait for some input
+        HoldIdlerSelector(); // put all movables on hold until the error screen gets resolved
     }
 }
 
diff --git a/src/logic/command_base.h b/src/logic/command_base.h
index 1a40ead..a8cb062 100644
--- a/src/logic/command_base.h
+++ b/src/logic/command_base.h
@@ -95,12 +95,19 @@ public:
     /// filament presence according to known sensors (FINDA+FSensor)
     static void InvalidateHomingAndFilamentState();
 
+    /// Put Idler and Selector on-hold - they shall not move (not even home) until ResumeIdlerSelector is called
+    static void HoldIdlerSelector();
+
+    /// Allow Idler and Selector to move/home again. Any move needs to be newly planned.
+    static void ResumeIdlerSelector();
+
 #ifndef UNITTEST
 protected:
 #endif
     /// @returns true if the slot/tool index is within specified range (0 - config::toolCount)
     /// If not, it returns false and sets the error to ErrorCode::INVALID_TOOL
-    bool CheckToolIndex(uint8_t index);
+    bool
+    CheckToolIndex(uint8_t index);
 
     /// Checks for errors of modules - that includes TMC errors, Idler and Selector errors and possibly more.
     /// The idea is to check blocking errors at one spot consistently.
diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp
index c5014ad..d7bb5a6 100644
--- a/src/logic/eject_filament.cpp
+++ b/src/logic/eject_filament.cpp
@@ -77,6 +77,7 @@ bool EjectFilament::StepInner() {
         mui::Event ev = mui::userInput.ConsumeEvent();
         switch (ev) {
         case mui::Event::Middle:
+            ResumeIdlerSelector();
             switch (error) {
             case ErrorCode::FILAMENT_EJECTED: // the user clicked "Done", we can finish the Eject operation
                 FinishedOK();
diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp
index 5a089e9..6a95981 100644
--- a/src/logic/load_filament.cpp
+++ b/src/logic/load_filament.cpp
@@ -122,6 +122,7 @@ bool LoadFilament::StepInner() {
         mui::Event ev = mui::userInput.ConsumeEvent();
         switch (ev) {
         case mui::Event::Middle: // try again the whole sequence
+            ResumeIdlerSelector();
             // however it depends on the state of FINDA - if it is on, we must perform unload first
             if (!mf::finda.Pressed()) {
                 Reset2(false);
diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
index d8711c6..4c05073 100644
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@@ -148,6 +148,8 @@ bool Idler::Step() {
             return false;
         }
         return true;
+    case OnHold:
+        return true; // just wait, do nothing!
     case TMCFailed:
         dbg_logic_P(PSTR("Idler Failed"));
     default:
diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h
index 376dea3..6d03a39 100644
--- a/src/modules/movable_base.h
+++ b/src/modules/movable_base.h
@@ -18,7 +18,8 @@ public:
         HomeForward,
         HomeBack,
         TMCFailed,
-        HomingFailed
+        HomingFailed,
+        OnHold,
     };
 
     /// Operation (Engage/Disengage/MoveToSlot) return values
@@ -72,6 +73,11 @@ public:
     /// iRun == 0 means set the default from config
     void SetCurrents(uint8_t iRun, uint8_t iHold);
 
+    /// Puts the movable on-hold
+    void HoldOn() { state = OnHold; }
+    /// Allows the movable to move/home again
+    void Resume() { state = Ready; }
+
 #ifndef UNITTEST
 protected:
 #endif
diff --git a/src/modules/pulley.cpp b/src/modules/pulley.cpp
index a0101ac..c8d3aa5 100644
--- a/src/modules/pulley.cpp
+++ b/src/modules/pulley.cpp
@@ -31,6 +31,8 @@ bool Pulley::Step() {
         return true;
     case Ready:
         return true;
+    case OnHold:
+        return true;
     case TMCFailed:
     default:
         return true;
diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
index e2c015c..549657f 100644
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@@ -124,6 +124,8 @@ bool Selector::Step() {
             return false;
         }
         return true;
+    case OnHold:
+        return true; // just wait, do nothing!
     case TMCFailed:
         dbg_logic_P(PSTR("Selector Failed"));
     default: