From a9200f73f5f71308c0d3c0806bf45756799b7086 Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Mon, 16 May 2022 16:24:49 +0200
Subject: [PATCH] Make SG work on Selector

surprisingly - it needed somewhat lower sensitivity, because the selector was triggering
stall guard signal even when decelerating (which is subject to separate investigation)
---
 src/config/config.h          |  2 +-
 src/logic/command_base.cpp   |  2 ++
 src/modules/movable_base.cpp | 16 +++++++++++-----
 3 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/src/config/config.h b/src/config/config.h
index 2556424..9cee3fa 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -131,7 +131,7 @@ static constexpr AxisConfig selector = {
     .iHold = 5, /// 99mA
     .stealth = false,
     .stepsPerUnit = (200 * 8 / 8.),
-    .sg_thrs = 3,
+    .sg_thrs = 5, // 6 looks like too much on my MMU
 };
 
 /// Selector motion limits
diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp
index 7294d20..8b87cea 100644
--- a/src/logic/command_base.cpp
+++ b/src/logic/command_base.cpp
@@ -85,12 +85,14 @@ bool CommandBase::WaitForOneModuleErrorRecovery(ErrorCode ec, modules::motion::M
             stateBeforeModuleFailed = state;
             errorBeforeModuleFailed = error;
             error = ec;
+            //            mui::userInput.Clear(); // @@TODO
             state = ProgressCode::ERRWaitingForUser; // such a situation always requires user's attention -> let the printer display an error screen
         }
 
         // are we already recovering an error - that would mean we got another one
         if (recoveringMovableErrorAxisMask) {
             error = ec;
+            //            mui::userInput.Clear(); // @@TODO
             state = ProgressCode::ERRWaitingForUser; // such a situation always requires user's attention -> let the printer display an error screen
         }
 
diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp
index 4d9d518..813ce49 100644
--- a/src/modules/movable_base.cpp
+++ b/src/modules/movable_base.cpp
@@ -2,6 +2,7 @@
 #include "movable_base.h"
 #include "globals.h"
 #include "motion.h"
+#include "leds.h"
 
 namespace modules {
 namespace motion {
@@ -37,18 +38,23 @@ void MovableBase::PerformMove() {
         // TMC2130 entered some error state, the planned move couldn't have been finished - result of operation is Failed
         tmcErrorFlags = mm::motion.DriverForAxis(axis).GetErrorFlags(); // save the failed state
         state = TMCFailed;
+    } else if (mm::motion.QueueEmpty(axis)) {
+        // move finished
+        //        ml::leds.SetMode(4, ml::red, ml::off); // @@TODO - temporary signal of the finished move
+        currentSlot = plannedSlot;
+        FinishMove();
+        state = Ready;
     } else if (SupportsHoming() && (!mg::globals.MotorsStealth()) && mm::motion.StallGuard(axis)) {
+        // Beware - the ordering of these if statements is important.
+        // We shall only check stallguard when motion queue is not empty for this axis - i.e. ! mm::motion.QueueEmpty(axis)
+        // Such a check has already been done in the previous else-if branch.
+        //        ml::leds.SetMode(4, ml::red, ml::on); // @@TODO - temporary signal of the stall guard
         // Axis stalled while moving - dangerous especially with the Selector
         // Checked only for axes which support homing (because we plan a homing move after the error is resolved to regain precise position)
         mm::motion.StallGuardReset(axis);
         mm::motion.AbortPlannedMoves(axis, true);
         // @@TODO move a bit back from where it came from to enable easier removal of whatever is blocking the axis
         state = MoveFailed;
-    } else if (mm::motion.QueueEmpty(axis)) {
-        // move finished
-        currentSlot = plannedSlot;
-        FinishMove();
-        state = Ready;
     }
 }