From 3646b8b1c051d114841ab0f3009e6fc2931b4b1a Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Tue, 7 Sep 2021 08:04:50 +0200
Subject: [PATCH] Change M0/M1 into a command

That implies changing motor's mode from SpreadCycle into StealtMode (or vice versa)
requires a stand still MMU with no other command (i.e. motor moves) being performed.
This elegantly solves the synchronization problem of TMC2130 mode change, as it results
in severe jerking while a motor is moving.

The change in protocol is minimal - M0/M1 first return `M0 A` (accepted) and another `Q0` then
returns `M0 F` (finished). The MK4 counterpart may ignore the additional report if necessary
as the mode change is done immediately (shortly after responding with `M0 A`)
---
 src/logic/CMakeLists.txt | 24 ++++++++++++------------
 src/logic/set_mode.cpp   | 17 +++++++++++++++++
 src/logic/set_mode.h     | 29 +++++++++++++++++++++++++++++
 src/main.cpp             | 14 ++++----------
 4 files changed, 62 insertions(+), 22 deletions(-)
 create mode 100644 src/logic/set_mode.cpp
 create mode 100644 src/logic/set_mode.h

diff --git a/src/logic/CMakeLists.txt b/src/logic/CMakeLists.txt
index ec8f87b..2b2122d 100644
--- a/src/logic/CMakeLists.txt
+++ b/src/logic/CMakeLists.txt
@@ -1,14 +1,14 @@
 target_sources(
   firmware
-  PRIVATE
-  command_base.cpp
-  cut_filament.cpp
-  eject_filament.cpp
-  feed_to_bondtech.cpp
-  feed_to_finda.cpp
-  load_filament.cpp
-  no_command.cpp
-  tool_change.cpp
-  unload_filament.cpp
-  unload_to_finda.cpp
-)
+  PRIVATE command_base.cpp
+          cut_filament.cpp
+          eject_filament.cpp
+          feed_to_bondtech.cpp
+          feed_to_finda.cpp
+          load_filament.cpp
+          no_command.cpp
+          set_mode.cpp
+          tool_change.cpp
+          unload_filament.cpp
+          unload_to_finda.cpp
+  )
diff --git a/src/logic/set_mode.cpp b/src/logic/set_mode.cpp
new file mode 100644
index 0000000..5da1043
--- /dev/null
+++ b/src/logic/set_mode.cpp
@@ -0,0 +1,17 @@
+#include "set_mode.h"
+#include "../modules/globals.h"
+#include "../modules/motion.h"
+
+namespace logic {
+
+SetMode setMode;
+
+void 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);
+    state = ProgressCode::OK;
+    error = ErrorCode::OK;
+}
+
+} // namespace logic
diff --git a/src/logic/set_mode.h b/src/logic/set_mode.h
new file mode 100644
index 0000000..7c3f380
--- /dev/null
+++ b/src/logic/set_mode.h
@@ -0,0 +1,29 @@
+#pragma once
+#include <stdint.h>
+#include "command_base.h"
+
+namespace logic {
+
+/// @brief Sets the mode of TMC2130 for all motors at once.
+/// In the original proposal, the M0/M1 message was declared as a query, since it can be processed immediately.
+/// The reality is a bit different - the TMC2130 driver cannot change from SpreadCycle into StealthMode while moving the motor,
+/// at least not without serious jerking in most cases.
+/// Therefore the M0/M1 messages were reconsidered into a command, because only one command at a time can be performed
+/// (regardless of how long it takes it to finish) - that implies no motor moves are being performed while M0/M1 is being applied.
+class SetMode : public CommandBase {
+public:
+    inline SetMode()
+        : CommandBase() {}
+
+    /// Restart the automaton
+    void 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)
+    bool StepInner() override { return true; }
+};
+
+/// The one and only instance of NoCommand state machine in the FW
+extern SetMode setMode;
+
+} // namespace logic
diff --git a/src/main.cpp b/src/main.cpp
index 6e96921..9fedbe2 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -26,6 +26,7 @@
 #include "logic/eject_filament.h"
 #include "logic/load_filament.h"
 #include "logic/no_command.h"
+#include "logic/set_mode.h"
 #include "logic/tool_change.h"
 #include "logic/unload_filament.h"
 
@@ -259,6 +260,9 @@ void PlanCommand(const mp::RequestMsg &rq) {
         case mp::RequestMsgCodes::Unload:
             currentCommand = &logic::unloadFilament;
             break;
+        case mp::RequestMsgCodes::Mode:
+            currentCommand = &logic::setMode;
+            break;
         default:
             currentCommand = &logic::noCommand;
             break;
@@ -271,12 +275,6 @@ void PlanCommand(const mp::RequestMsg &rq) {
     }
 }
 
-void SetMode(uint8_t m) {
-    mg::globals.SetMotorsMode(m != 0); // remember the last mode set
-    // distribute the mode to all motors immediately
-    mm::motion.SetMode((m == 0) ? mm::Normal : mm::Stealth);
-}
-
 void ProcessRequestMsg(const mp::RequestMsg &rq) {
     switch (rq.code) {
     case mp::RequestMsgCodes::Button:
@@ -287,10 +285,6 @@ void ProcessRequestMsg(const mp::RequestMsg &rq) {
         // immediately report FINDA status
         ReportFINDA(rq);
         break;
-    case mp::RequestMsgCodes::Mode:
-        // immediately switch to normal/stealth as requested
-        SetMode(rq.value);
-        break;
     case mp::RequestMsgCodes::Query:
         // immediately report progress of currently running command
         ReportRunningCommand();