From 57f488738bd1cf3830910758e4332c39ce819b46 Mon Sep 17 00:00:00 2001
From: MangaValk <patrickvalkmanga@hotmail.com>
Date: Sun, 8 Mar 2020 22:13:41 +0100
Subject: [PATCH] [ToolChanger] Lock the current tool at power-up (#17093)

---
 Marlin/src/MarlinCore.cpp         |  6 +++++-
 Marlin/src/module/tool_change.cpp | 13 +++++++++----
 Marlin/src/module/tool_change.h   |  4 ++++
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp
index 39287d4233..73a6cfc6b9 100644
--- a/Marlin/src/MarlinCore.cpp
+++ b/Marlin/src/MarlinCore.cpp
@@ -165,7 +165,7 @@
   #include "feature/fanmux.h"
 #endif
 
-#if DO_SWITCH_EXTRUDER || ANY(SWITCHING_NOZZLE, PARKING_EXTRUDER, MAGNETIC_PARKING_EXTRUDER, ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
+#if DO_SWITCH_EXTRUDER || ANY(SWITCHING_NOZZLE, PARKING_EXTRUDER, MAGNETIC_PARKING_EXTRUDER, ELECTROMAGNETIC_SWITCHING_TOOLHEAD, SWITCHING_TOOLHEAD)
   #include "module/tool_change.h"
 #endif
 
@@ -1110,6 +1110,10 @@ void setup() {
     pe_solenoid_init();
   #endif
 
+  #if ENABLED(SWITCHING_TOOLHEAD)
+    swt_init();
+  #endif
+
   #if ENABLED(ELECTROMAGNETIC_SWITCHING_TOOLHEAD)
     est_init();
   #endif
diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp
index 021878c1ea..2a8fd42198 100644
--- a/Marlin/src/module/tool_change.cpp
+++ b/Marlin/src/module/tool_change.cpp
@@ -368,11 +368,16 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
 
 #if ENABLED(SWITCHING_TOOLHEAD)
 
+  inline void swt_lock(const bool locked=true) {
+    const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
+    MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]);
+  }
+
+  void swt_init() { swt_lock(); }
+
   inline void switching_toolhead_tool_change(const uint8_t new_tool, bool no_move/*=false*/) {
     if (no_move) return;
 
-    constexpr uint16_t angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
-
     constexpr float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS;
     const float placexpos = toolheadposx[active_extruder],
                 grabxpos = toolheadposx[new_tool];
@@ -406,7 +411,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
 
     planner.synchronize();
     if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(2) Unlock and Place Toolhead");
-    MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[1]);
+    swt_lock(false);
     safe_delay(500);
 
     current_position.y = SWITCHING_TOOLHEAD_Y_POS;
@@ -451,7 +456,7 @@ inline void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_a
     // Wait for move to finish, pause 0.2s, move servo, pause 0.5s
     planner.synchronize();
     safe_delay(200);
-    MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[0]);
+    swt_lock();
     safe_delay(500);
 
     current_position.y -= SWITCHING_TOOLHEAD_Y_CLEAR;
diff --git a/Marlin/src/module/tool_change.h b/Marlin/src/module/tool_change.h
index 025d2997d1..17c173d052 100644
--- a/Marlin/src/module/tool_change.h
+++ b/Marlin/src/module/tool_change.h
@@ -97,6 +97,10 @@
   void est_init();
 #endif
 
+#if ENABLED(SWITCHING_TOOLHEAD)
+  void swt_init();
+#endif
+
 /**
  * Perform a tool-change, which may result in moving the
  * previous tool out of the way and the new tool into place.