From 9ecdd1f4c73c4302af465c2566c48d5618fd76c6 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Tue, 26 Dec 2017 22:16:13 -0600
Subject: [PATCH] Multi extruder support for M600 and LCD

---
 Marlin/src/gcode/feature/pause/M600.cpp | 40 +++++++----
 Marlin/src/lcd/ultralcd.cpp             | 94 ++++++++++++++++++++++---
 2 files changed, 113 insertions(+), 21 deletions(-)

diff --git a/Marlin/src/gcode/feature/pause/M600.cpp b/Marlin/src/gcode/feature/pause/M600.cpp
index 7b6bfb912a..c1793f6faf 100644
--- a/Marlin/src/gcode/feature/pause/M600.cpp
+++ b/Marlin/src/gcode/feature/pause/M600.cpp
@@ -24,14 +24,15 @@
 
 #if ENABLED(ADVANCED_PAUSE_FEATURE)
 
-#include "../../../feature/pause.h"
-
 #include "../../gcode.h"
+#include "../../../feature/pause.h"
 #include "../../../module/motion.h"
-#include "../../parser.h"
-
 #include "../../../module/printcounter.h"
 
+#if EXTRUDERS > 1
+  #include "../../../module/tool_change.h"
+#endif
+
 /**
  * M600: Pause for filament change
  *
@@ -42,6 +43,7 @@
  *  U[distance] - Retract distance for removal (negative value) (manual reload)
  *  L[distance] - Extrude distance for insertion (positive value) (manual reload)
  *  B[count]    - Number of times to beep, -1 for indefinite (if equipped with a buzzer)
+ *  T[toolhead] - Select extruder for filament change
  *
  *  Default values are used for omitted arguments.
  *
@@ -54,6 +56,18 @@ void GcodeSuite::M600() {
     if (axis_unhomed_error()) home_all_axes();
   #endif
 
+  #if EXTRUDERS > 1
+    // Change toolhead if specified
+    uint8_t active_extruder_before_filament_change = -1;
+    if (parser.seen('T')) {
+      const uint8_t extruder = parser.value_byte();
+      if (active_extruder != extruder) {
+        active_extruder_before_filament_change = active_extruder;
+        tool_change(extruder, 0, true);
+      }
+    }
+  #endif
+
   // Initial retract before move to filament change position
   const float retract = parser.seen('E') ? parser.value_axis_units(E_AXIS) : 0
     #ifdef PAUSE_PARK_RETRACT_LENGTH
@@ -61,16 +75,12 @@ void GcodeSuite::M600() {
     #endif
   ;
 
-  // Lift Z axis
-  if (parser.seenval('Z'))
-    park_point.z = parser.linearval('Z');
-
   // Move XY axes to filament change position or given position
-  if (parser.seenval('X'))
-    park_point.x = parser.linearval('X');
+  if (parser.seenval('X')) park_point.x = parser.linearval('X');
+  if (parser.seenval('Y')) park_point.y = parser.linearval('Y');
 
-  if (parser.seenval('Y'))
-    park_point.y = parser.linearval('Y');
+  // Lift Z axis
+  if (parser.seenval('Z')) park_point.z = parser.linearval('Z');
 
   #if HOTENDS > 1 && DISABLED(DUAL_X_CARRIAGE)
     park_point.x += (active_extruder ? hotend_offset[X_AXIS][active_extruder] : 0);
@@ -106,6 +116,12 @@ void GcodeSuite::M600() {
     resume_print(load_length, ADVANCED_PAUSE_EXTRUDE_LENGTH, beep_count);
   }
 
+  #if EXTRUDERS > 1
+    // Restore toolhead if it was changed
+    if (active_extruder_before_filament_change >= 0)
+      tool_change(active_extruder_before_filament_change, 0, true);
+  #endif
+
   // Resume the print job timer if it was running
   if (job_running) print_job_timer.start();
 }
diff --git a/Marlin/src/lcd/ultralcd.cpp b/Marlin/src/lcd/ultralcd.cpp
index 44ff950198..48db56c8ba 100644
--- a/Marlin/src/lcd/ultralcd.cpp
+++ b/Marlin/src/lcd/ultralcd.cpp
@@ -1246,11 +1246,14 @@ void kill_screen(const char* lcd_msg) {
 
   #if ENABLED(ADVANCED_PAUSE_FEATURE)
 
-    void lcd_enqueue_filament_change() {
+    void lcd_enqueue_filament_change(
+      #if EXTRUDERS > 1
+        const uint8_t extruder
+      #endif
+    ) {
 
       #if ENABLED(PREVENT_COLD_EXTRUSION)
-        if (!DEBUGGING(DRYRUN) && !thermalManager.allow_cold_extrude &&
-            thermalManager.degTargetHotend(active_extruder) < thermalManager.extrude_min_temp) {
+        if (!DEBUGGING(DRYRUN) && thermalManager.tooColdToExtrude(active_extruder)) {
           lcd_save_previous_screen();
           lcd_goto_screen(lcd_advanced_pause_toocold_menu);
           return;
@@ -1258,9 +1261,42 @@ void kill_screen(const char* lcd_msg) {
       #endif
 
       lcd_advanced_pause_show_message(ADVANCED_PAUSE_MESSAGE_INIT);
-      enqueue_and_echo_commands_P(PSTR("M600 B0"));
+
+      #if EXTRUDERS <= 1
+        enqueue_and_echo_commands_P(PSTR("M600 B0"));
+      #else
+        char *command_M600;
+        switch (extruder) {
+          case 0: command_M600 = PSTR("M600 B0 T0"); break;
+          case 1: command_M600 = PSTR("M600 B0 T1"); break;
+          #if EXTRUDERS > 2
+            case 2: command_M600 = PSTR("M600 B0 T2"); break;
+            #if EXTRUDERS > 3
+              case 3: command_M600 = PSTR("M600 B0 T3"); break;
+              #if EXTRUDERS > 4
+                case 4: command_M600 = PSTR("M600 B0 T4"); break;
+              #endif // EXTRUDERS > 4
+            #endif // EXTRUDERS > 3
+          #endif // EXTRUDERS > 2
+        }
+        enqueue_and_echo_commands_P(command_M600);
+      #endif // EXTRUDERS > 1
     }
 
+    #if EXTRUDERS > 1
+      void lcd_enqueue_filament_change_e0() { lcd_enqueue_filament_change(0); }
+      void lcd_enqueue_filament_change_e1() { lcd_enqueue_filament_change(1); }
+      #if EXTRUDERS > 2
+        void lcd_enqueue_filament_change_e2() { lcd_enqueue_filament_change(2); }
+        #if EXTRUDERS > 3
+          void lcd_enqueue_filament_change_e3() { lcd_enqueue_filament_change(3); }
+          #if EXTRUDERS > 4
+            void lcd_enqueue_filament_change_e4() { lcd_enqueue_filament_change(4); }
+          #endif // EXTRUDERS > 4
+        #endif // EXTRUDERS > 3
+      #endif // EXTRUDERS > 2
+    #endif // EXTRUDERS > 1
+
   #endif // ADVANCED_PAUSE_FEATURE
 
   // First Fan Speed title in "Tune" and "Control>Temperature" menus
@@ -1404,8 +1440,27 @@ void kill_screen(const char* lcd_msg) {
     // Change filament
     //
     #if ENABLED(ADVANCED_PAUSE_FEATURE)
-      if (!thermalManager.tooColdToExtrude(active_extruder))
-        MENU_ITEM(function, MSG_FILAMENTCHANGE, lcd_enqueue_filament_change);
+      #if EXTRUDERS > 1
+        if (!thermalManager.tooColdToExtrude(0))
+          MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E1, lcd_enqueue_filament_change_e0);
+        if (!thermalManager.tooColdToExtrude(1))
+          MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E2, lcd_enqueue_filament_change_e1);
+        #if EXTRUDERS > 2
+          if (!thermalManager.tooColdToExtrude(2))
+            MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E3, lcd_enqueue_filament_change_e2);
+          #if EXTRUDERS > 3
+            if (!thermalManager.tooColdToExtrude(3))
+              MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E4, lcd_enqueue_filament_change_e3);
+            #if EXTRUDERS > 4
+              if (!thermalManager.tooColdToExtrude(4))
+                MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E5, lcd_enqueue_filament_change_e4);
+            #endif // EXTRUDERS > 4
+          #endif // EXTRUDERS > 3
+        #endif // EXTRUDERS > 2
+      #else
+        if (!thermalManager.tooColdToExtrude(active_extruder))
+          MENU_ITEM(function, MSG_FILAMENTCHANGE, lcd_enqueue_filament_change);
+      #endif
     #endif
 
     END_MENU();
@@ -2595,9 +2650,30 @@ void kill_screen(const char* lcd_msg) {
     // Change filament
     //
     #if ENABLED(ADVANCED_PAUSE_FEATURE)
-      if (!thermalManager.tooColdToExtrude(active_extruder) && !IS_SD_FILE_OPEN)
-        MENU_ITEM(function, MSG_FILAMENTCHANGE, lcd_enqueue_filament_change);
-    #endif
+      if (!IS_SD_FILE_OPEN) {
+        #if EXTRUDERS > 1
+          if (!thermalManager.tooColdToExtrude(0))
+            MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E1, lcd_enqueue_filament_change_e0);
+          if (!thermalManager.tooColdToExtrude(1))
+            MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E2, lcd_enqueue_filament_change_e1);
+          #if EXTRUDERS > 2
+            if (!thermalManager.tooColdToExtrude(2))
+              MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E3, lcd_enqueue_filament_change_e2);
+            #if EXTRUDERS > 3
+              if (!thermalManager.tooColdToExtrude(3))
+                MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E4, lcd_enqueue_filament_change_e3);
+              #if EXTRUDERS > 4
+                if (!thermalManager.tooColdToExtrude(4))
+                  MENU_ITEM(function, MSG_FILAMENTCHANGE " " MSG_E5, lcd_enqueue_filament_change_e4);
+              #endif // EXTRUDERS > 4
+            #endif // EXTRUDERS > 3
+          #endif // EXTRUDERS > 2
+        #else
+          if (!thermalManager.tooColdToExtrude(active_extruder))
+            MENU_ITEM(function, MSG_FILAMENTCHANGE, lcd_enqueue_filament_change);
+        #endif
+      }
+    #endif // ADVANCED_PAUSE_FEATURE
 
     #if TEMP_SENSOR_0 != 0