From 178aeb79c886ae898317e01b05202c185cd1a03b Mon Sep 17 00:00:00 2001
From: gralco <kuzmenko@alephobjects.com>
Date: Tue, 12 Apr 2016 08:40:57 -0600
Subject: [PATCH] Implementation of M190 bed temp hysteresis

---
 Marlin/Configuration.h |  6 +++++-
 Marlin/Marlin_main.cpp | 45 ++++++++++++++++++++++++++++++++++++++----
 2 files changed, 46 insertions(+), 5 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index d0a49aade3..a7aedd8ad5 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -192,11 +192,15 @@
 //#define TEMP_SENSOR_1_AS_REDUNDANT
 #define MAX_REDUNDANT_TEMP_SENSOR_DIFF 10
 
-// Actual temperature must be close to target for this long before M109 returns success
+// Actual temperature must be close to target for this long before M109/M190 returns success
 #define TEMP_RESIDENCY_TIME 10  // (seconds)
 #define TEMP_HYSTERESIS 3       // (degC) range of +/- temperatures considered "close" to the target one
 #define TEMP_WINDOW     1       // (degC) Window around target to start the residency timer x degC early.
 
+#define TEMP_BED_RESIDENCY_TIME 10  // (seconds)
+#define TEMP_BED_HYSTERESIS 3       // (degC) range of +/- temperatures considered "close" to the target one
+#define TEMP_BED_WINDOW     1       // (degC) Window around target to start the residency timer x degC early.
+
 // The minimal temperature defines the temperature below which the heater will not be enabled It is used
 // to check that the wiring to the thermistor is not broken.
 // Otherwise this would lead to the heater being powered on all the time.
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d1019b34b3..571e8d2d5b 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -4389,20 +4389,57 @@ inline void gcode_M109() {
     // Exit if S<lower>, continue if S<higher>, R<lower>, or R<higher>
     if (no_wait_for_cooling && wants_to_cool) return;
 
+    #ifdef TEMP_BED_RESIDENCY_TIME
+      millis_t residency_start_ms = 0;
+      // Loop until the temperature has stabilized
+      #define TEMP_BED_CONDITIONS (!residency_start_ms || PENDING(now, residency_start_ms + (TEMP_BED_RESIDENCY_TIME) * 1000UL))
+    #else
+      // Loop until the temperature is very close target
+      #define TEMP_BED_CONDITIONS (wants_to_cool ? isCoolingBed() : isHeatingBed())
+    #endif //TEMP_BED_RESIDENCY_TIME
+
     cancel_heatup = false;
-    millis_t next_temp_ms = 0;
+    millis_t now, next_temp_ms = 0;
 
     // Wait for temperature to come close enough
     do {
-      millis_t now = millis();
+      now = millis();
       if (ELAPSED(now, next_temp_ms)) { //Print Temp Reading every 1 second while heating up.
         next_temp_ms = now + 1000UL;
         print_heaterstates();
-        SERIAL_EOL;
+        #ifdef TEMP_BED_RESIDENCY_TIME
+          SERIAL_PROTOCOLPGM(" W:");
+          if (residency_start_ms) {
+            long rem = (((TEMP_BED_RESIDENCY_TIME) * 1000UL) - (now - residency_start_ms)) / 1000UL;
+            SERIAL_PROTOCOLLN(rem);
+          }
+          else {
+            SERIAL_PROTOCOLLNPGM("?");
+          }
+        #else
+          SERIAL_EOL;
+        #endif
       }
+
       idle();
       refresh_cmd_timeout(); // to prevent stepper_inactive_time from running out
-    } while (!cancel_heatup && (wants_to_cool ? isCoolingBed() : isHeatingBed()));
+
+      #ifdef TEMP_BED_RESIDENCY_TIME
+
+        float temp_diff = fabs(degBed() - degTargetBed());
+
+        if (!residency_start_ms) {
+          // Start the TEMP_BED_RESIDENCY_TIME timer when we reach target temp for the first time.
+          if (temp_diff < TEMP_BED_WINDOW) residency_start_ms = millis();
+        }
+        else if (temp_diff > TEMP_BED_HYSTERESIS) {
+          // Restart the timer whenever the temperature falls outside the hysteresis.
+          residency_start_ms = millis();
+        }
+
+      #endif //TEMP_BED_RESIDENCY_TIME
+
+    } while (!cancel_heatup && TEMP_BED_CONDITIONS);
     LCD_MESSAGEPGM(MSG_BED_DONE);
   }