From 1e6194a9b989a5a6dc8f49e92890463a0674b9fb Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Tue, 31 Aug 2021 07:51:28 +0200
Subject: [PATCH] Add support for M0/M1 messages

... route the messages into modules::motion's SetMode of all axes.
---
 src/main.cpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/src/main.cpp b/src/main.cpp
index e397488..b39469a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -271,6 +271,13 @@ void PlanCommand(const mp::RequestMsg &rq) {
     }
 }
 
+void SetMode(uint8_t m) {
+    mm::MotorMode mode = (m == 0) ? mm::Normal : mm::Stealth;
+    mm::motion.SetMode(mm::Pulley, mode);
+    mm::motion.SetMode(mm::Selector, mode);
+    mm::motion.SetMode(mm::Idler, mode);
+}
+
 void ProcessRequestMsg(const mp::RequestMsg &rq) {
     switch (rq.code) {
     case mp::RequestMsgCodes::Button:
@@ -283,7 +290,7 @@ void ProcessRequestMsg(const mp::RequestMsg &rq) {
         break;
     case mp::RequestMsgCodes::Mode:
         // immediately switch to normal/stealth as requested
-        // modules::motion::SetMode();
+        SetMode(rq.value);
         break;
     case mp::RequestMsgCodes::Query:
         // immediately report progress of currently running command