From 88d7f4d7be744ecfecdb531cb48ae42a02403cef Mon Sep 17 00:00:00 2001
From: Trocololo <isanchez@neuda.net>
Date: Sat, 26 Sep 2020 03:28:01 +0200
Subject: [PATCH] MMU2 S Mode spins the BMG gears during C0 (#19429)

---
 Marlin/src/feature/mmu2/mmu2.cpp | 67 +++++++++++++++++++++-----------
 1 file changed, 44 insertions(+), 23 deletions(-)

diff --git a/Marlin/src/feature/mmu2/mmu2.cpp b/Marlin/src/feature/mmu2/mmu2.cpp
index 35f2db45a907..31b179a3e5e9 100644
--- a/Marlin/src/feature/mmu2/mmu2.cpp
+++ b/Marlin/src/feature/mmu2/mmu2.cpp
@@ -54,6 +54,8 @@ MMU2 mmu2;
 #define MMU_CMD_TIMEOUT 45000UL // 45s timeout for mmu commands (except P0)
 #define MMU_P0_TIMEOUT 3000UL   // Timeout for P0 command: 3seconds
 
+#define MMU2_COMMAND(S) tx_str_P(PSTR(S "\n"))
+
 #if ENABLED(MMU_EXTRUDER_SENSOR)
   uint8_t mmu_idl_sens = 0;
   static bool mmu_loading_flag = false;
@@ -152,7 +154,7 @@ void MMU2::reset() {
     safe_delay(20);
     WRITE(MMU2_RST_PIN, HIGH);
   #else
-    tx_str_P(PSTR("X0\n")); // Send soft reset
+    MMU2_COMMAND("X0"); // Send soft reset
   #endif
 }
 
@@ -175,9 +177,7 @@ void MMU2::mmu_loop() {
         DEBUG_ECHOLNPGM("MMU => 'start'");
         DEBUG_ECHOLNPGM("MMU <= 'S1'");
 
-        // send "read version" request
-        tx_str_P(PSTR("S1\n"));
-
+        MMU2_COMMAND("S1");   // Read Version
         state = -2;
       }
       else if (millis() > 3000000) {
@@ -192,7 +192,7 @@ void MMU2::mmu_loop() {
 
         DEBUG_ECHOLNPAIR("MMU => ", version, "\nMMU <= 'S2'");
 
-        tx_str_P(PSTR("S2\n")); // read build number
+        MMU2_COMMAND("S2");   // Read Build Number
         state = -3;
       }
       break;
@@ -208,13 +208,13 @@ void MMU2::mmu_loop() {
         #if ENABLED(MMU2_MODE_12V)
           DEBUG_ECHOLNPGM("MMU <= 'M1'");
 
-          tx_str_P(PSTR("M1\n")); // switch to stealth mode
+          MMU2_COMMAND("M1");   // Stealth Mode
           state = -5;
 
         #else
           DEBUG_ECHOLNPGM("MMU <= 'P0'");
 
-          tx_str_P(PSTR("P0\n")); // read finda
+          MMU2_COMMAND("P0");   // Read FINDA
           state = -4;
         #endif
       }
@@ -228,7 +228,7 @@ void MMU2::mmu_loop() {
 
         DEBUG_ECHOLNPGM("MMU <= 'P0'");
 
-        tx_str_P(PSTR("P0\n")); // read finda
+        MMU2_COMMAND("P0");   // Read FINDA
         state = -4;
       }
       break;
@@ -266,14 +266,14 @@ void MMU2::mmu_loop() {
         else if (cmd == MMU_CMD_C0) {
           // continue loading
           DEBUG_ECHOLNPGM("MMU <= 'C0'");
-          tx_str_P(PSTR("C0\n"));
+          MMU2_COMMAND("C0");
           state = 3; // wait for response
         }
         else if (cmd == MMU_CMD_U0) {
           // unload current
           DEBUG_ECHOLNPGM("MMU <= 'U0'");
 
-          tx_str_P(PSTR("U0\n"));
+          MMU2_COMMAND("U0");
           state = 3; // wait for response
         }
         else if (WITHIN(cmd, MMU_CMD_E0, MMU_CMD_E4)) {
@@ -286,7 +286,7 @@ void MMU2::mmu_loop() {
         else if (cmd == MMU_CMD_R0) {
           // recover after eject
           DEBUG_ECHOLNPGM("MMU <= 'R0'");
-          tx_str_P(PSTR("R0\n"));
+          MMU2_COMMAND("R0");
           state = 3; // wait for response
         }
         else if (WITHIN(cmd, MMU_CMD_F0, MMU_CMD_F4)) {
@@ -303,8 +303,7 @@ void MMU2::mmu_loop() {
         cmd = MMU_CMD_NONE;
       }
       else if (ELAPSED(millis(), prev_P0_request + 300)) {
-        // read FINDA
-        tx_str_P(PSTR("P0\n"));
+        MMU2_COMMAND("P0"); // Read FINDA
         state = 2; // wait for response
       }
 
@@ -332,19 +331,32 @@ void MMU2::mmu_loop() {
       #if ENABLED(MMU_EXTRUDER_SENSOR)
         if (mmu_idl_sens) {
           if (FILAMENT_PRESENT() && mmu_loading_flag) {
-            DEBUG_ECHOLNPGM("MMU <= 'A'\n");
-            tx_str_P(PSTR("A\n")); // send 'abort' request
+            DEBUG_ECHOLNPGM("MMU <= 'A'");
+            MMU2_COMMAND("A"); // send 'abort' request
             mmu_idl_sens = 0;
-            DEBUG_ECHOLNPGM("MMU IDLER_SENSOR = 0 - ABORT\n");
+            DEBUG_ECHOLNPGM("MMU IDLER_SENSOR = 0 - ABORT");
           }
         }
       #endif
 
       if (rx_ok()) {
-        DEBUG_ECHOLNPGM("MMU => 'ok'");
-        ready = true;
-        state = 1;
-        last_cmd = MMU_CMD_NONE;
+        // response to C0 mmu command in PRUSA_MMU2_S_MODE
+        bool can_reset = true;
+        if (ENABLED(PRUSA_MMU2_S_MODE) && last_cmd == MMU_CMD_C0) {
+          if (!mmu2s_triggered) {
+            can_reset = false;
+            // MMU ok received but filament sensor not triggered, retrying...
+            DEBUG_ECHOLNPGM("MMU => 'ok' (filament not present in gears)");
+            DEBUG_ECHOLNPGM("MMU <= 'C0' (keep trying)");
+            MMU2_COMMAND("C0");
+          }
+        }
+        if (can_reset) {
+          DEBUG_ECHOLNPGM("MMU => 'ok'");
+          ready = true;
+          state = 1;
+          last_cmd = MMU_CMD_NONE;
+        }
       }
       else if (ELAPSED(millis(), prev_request + MMU_CMD_TIMEOUT)) {
         // resend request after timeout
@@ -862,9 +874,18 @@ void MMU2::filament_runout() {
 
   void MMU2::check_filament() {
     const bool present = FILAMENT_PRESENT();
-    if (present && !mmu2s_triggered) {
-      DEBUG_ECHOLNPGM("MMU <= 'A'");
-      tx_str_P(PSTR("A\n"));
+    if (cmd == MMU_CMD_NONE && last_cmd == MMU_CMD_C0) {
+      if (present && !mmu2s_triggered) {
+        DEBUG_ECHOLNPGM("MMU <= 'A'");
+        tx_str_P(PSTR("A\n"));
+      }
+      // Slowly spin the extruder during C0
+      else {
+        while (planner.movesplanned() < 3) {
+          current_position.e += 0.25;
+          line_to_current_position(MMM_TO_MMS(120));
+        }
+      }
     }
     mmu2s_triggered = present;
   }