From d814e468d2f2dad353cefe59a8719c7eb112ae1f Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Mon, 1 May 2017 02:27:52 -0500
Subject: [PATCH] Fix set_heaters_for_bltouch and usage

- Only call `set_heaters_for_bltouch` when actually deploying/stowing
- Clear `next_emi_protection` when re-enabling heaters
---
 Marlin/Marlin_main.cpp | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 2fa5dba6da..aff06544c7 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -2080,7 +2080,7 @@ static void clean_up_after_endstop_or_probe_move() {
     #if ENABLED(BLTOUCH_HEATERS_OFF)
 
       void set_heaters_for_bltouch(const bool deploy) {
-        static bool heaters_were_disabled = false;
+        static int8_t heaters_were_disabled = 0;
         static millis_t next_emi_protection;
         static float temps_at_entry[HOTENDS];
 
@@ -2105,6 +2105,7 @@ static void clean_up_after_endstop_or_probe_move() {
           #endif
         }
         else {
+          next_emi_protection = 0;
           HOTEND_LOOP() thermalManager.setTargetHotend(temps_at_entry[e], e);
           #if HAS_TEMP_BED
             thermalManager.setTargetBed(bed_temp_at_entry);
@@ -2115,9 +2116,6 @@ static void clean_up_after_endstop_or_probe_move() {
     #endif // BLTOUCH_HEATERS_OFF
 
     void set_bltouch_deployed(const bool deploy) {
-      #if ENABLED(BLTOUCH_HEATERS_OFF)
-        set_heaters_for_bltouch(deploy);
-      #endif
       if (deploy && TEST_BLTOUCH()) {      // If BL-Touch says it's triggered
         bltouch_command(BLTOUCH_RESET);    //  try to reset it.
         bltouch_command(BLTOUCH_DEPLOY);   // Also needs to deploy and stow to
@@ -2131,6 +2129,9 @@ static void clean_up_after_endstop_or_probe_move() {
           stop();                          // punt!
         }
       }
+      #if ENABLED(BLTOUCH_HEATERS_OFF)
+        set_heaters_for_bltouch(deploy);
+      #endif
       bltouch_command(deploy ? BLTOUCH_DEPLOY : BLTOUCH_STOW);
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) {
@@ -2153,12 +2154,12 @@ static void clean_up_after_endstop_or_probe_move() {
       }
     #endif
 
+    if (endstops.z_probe_enabled == deploy) return false;
+
     #if ENABLED(BLTOUCH) && ENABLED(BLTOUCH_HEATERS_OFF)
       set_heaters_for_bltouch(deploy);
     #endif
 
-    if (endstops.z_probe_enabled == deploy) return false;
-
     // Make room for probe
     do_probe_raise(_Z_CLEARANCE_DEPLOY_PROBE);