From 0cc6955ab72ea03663fa52b5a4f9ecddf40d6bba Mon Sep 17 00:00:00 2001
From: InsanityAutomation <d.menzel@insanityautomation.com>
Date: Sat, 9 Oct 2021 13:58:25 -0400
Subject: [PATCH] BLTouch High Speed EEPROM Control

---
 Marlin/Configuration_adv.h                 |  2 ++
 Marlin/src/feature/bltouch.cpp             |  3 ++-
 Marlin/src/feature/bltouch.h               |  7 ++-----
 Marlin/src/gcode/bedlevel/G35.cpp          |  3 ++-
 Marlin/src/gcode/calibrate/G34_M422.cpp    |  2 +-
 Marlin/src/gcode/probe/M401_M402.cpp       | 12 ++++++++++++
 Marlin/src/lcd/language/language_en.h      |  1 +
 Marlin/src/lcd/menu/menu_bed_corners.cpp   | 15 ++++++++-------
 Marlin/src/lcd/menu/menu_configuration.cpp |  1 +
 Marlin/src/lcd/menu/menu_tramming.cpp      |  3 ++-
 Marlin/src/module/motion.cpp               |  9 +++++----
 Marlin/src/module/probe.cpp                | 19 +++++++++++--------
 Marlin/src/module/settings.cpp             | 22 ++++++++++++++++------
 13 files changed, 65 insertions(+), 34 deletions(-)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 8e1f673c87..0b01ec0d61 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -857,6 +857,8 @@
    * Danger: Disable if your probe sometimes fails. Only suitable for stable well-adjusted systems.
    * This feature was designed for Deltabots with very fast Z moves; however, higher speed Cartesians
    * might be able to use it. If the machine can't raise Z fast enough the BLTouch may go into ALARM.
+   *
+   * This sets the default state. Can be changed through the SCD
    */
   //#define BLTOUCH_HS_MODE
 
diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp
index 49a10f62b1..fbe6012bbe 100644
--- a/Marlin/src/feature/bltouch.cpp
+++ b/Marlin/src/feature/bltouch.cpp
@@ -28,7 +28,8 @@
 
 BLTouch bltouch;
 
-bool BLTouch::last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+bool BLTouch::last_written_mode;  // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+bool BLTouch::bltouch_high_speed; // Initialized by settings.load, 0 = Low Speed; 1 = High Speed
 
 #include "../module/servo.h"
 #include "../module/probe.h"
diff --git a/Marlin/src/feature/bltouch.h b/Marlin/src/feature/bltouch.h
index 9ecccb4256..7d9a0bb126 100644
--- a/Marlin/src/feature/bltouch.h
+++ b/Marlin/src/feature/bltouch.h
@@ -23,10 +23,6 @@
 
 #include "../inc/MarlinConfigPre.h"
 
-#if DISABLED(BLTOUCH_HS_MODE)
-  #define BLTOUCH_SLOW_MODE 1
-#endif
-
 // BLTouch commands are sent as servo angles
 typedef unsigned char BLTCommand;
 
@@ -71,7 +67,8 @@ typedef unsigned char BLTCommand;
 class BLTouch {
 public:
   static void init(const bool set_voltage=false);
-  static bool last_written_mode; // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+  static bool last_written_mode;  // Initialized by settings.load, 0 = Open Drain; 1 = 5V Drain
+  static bool bltouch_high_speed; // Initialized by settings.load, 0 = Low Speed; 1 = High Speed
 
   // DEPLOY and STOW are wrapped for error handling - these are used by homing and by probing
   static bool deploy()              { return deploy_proc(); }
diff --git a/Marlin/src/gcode/bedlevel/G35.cpp b/Marlin/src/gcode/bedlevel/G35.cpp
index e45e18b7fb..08539f8966 100644
--- a/Marlin/src/gcode/bedlevel/G35.cpp
+++ b/Marlin/src/gcode/bedlevel/G35.cpp
@@ -102,7 +102,8 @@ void GcodeSuite::G35() {
     // In BLTOUCH HS mode, the probe travels in a deployed state.
     // Users of G35 might have a badly misaligned bed, so raise Z by the
     // length of the deployed pin (BLTOUCH stroke < 7mm)
-    do_blocking_move_to_z(SUM_TERN(BLTOUCH_HS_MODE, Z_CLEARANCE_BETWEEN_PROBES, 7));
+    float z_raise_probe = Z_CLEARANCE_BETWEEN_PROBES if(bltouch.bltouch_high_speed) + 7;
+    do_blocking_move_to_z(z_raise_probe);
     const float z_probed_height = probe.probe_at_point(tramming_points[i], PROBE_PT_RAISE, 0, true);
 
     if (isnan(z_probed_height)) {
diff --git a/Marlin/src/gcode/calibrate/G34_M422.cpp b/Marlin/src/gcode/calibrate/G34_M422.cpp
index 8f4eab2c97..5b70dc588e 100644
--- a/Marlin/src/gcode/calibrate/G34_M422.cpp
+++ b/Marlin/src/gcode/calibrate/G34_M422.cpp
@@ -149,7 +149,7 @@ void GcodeSuite::G34() {
       // In BLTOUCH HS mode, the probe travels in a deployed state.
       // Users of G34 might have a badly misaligned bed, so raise Z by the
       // length of the deployed pin (BLTOUCH stroke < 7mm)
-      #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * BOTH(BLTOUCH, BLTOUCH_HS_MODE))
+      #define Z_BASIC_CLEARANCE (Z_CLEARANCE_BETWEEN_PROBES + 7.0f * bltouch.bltouch_high_speed)
 
       // Compute a worst-case clearance height to probe from. After the first
       // iteration this will be re-calculated based on the actual bed position
diff --git a/Marlin/src/gcode/probe/M401_M402.cpp b/Marlin/src/gcode/probe/M401_M402.cpp
index bd9bb44c40..aef34f3a1e 100644
--- a/Marlin/src/gcode/probe/M401_M402.cpp
+++ b/Marlin/src/gcode/probe/M401_M402.cpp
@@ -27,11 +27,23 @@
 #include "../gcode.h"
 #include "../../module/motion.h"
 #include "../../module/probe.h"
+#if ENABLED(BLTOUCH)
+  #include "../../feature/bltouch.h"
+#endif
 
 /**
  * M401: Deploy and activate the Z probe
  */
 void GcodeSuite::M401() {
+  #if ENABLED(BLTOUCH)
+    const bool seen_S = parser.seen('S'),
+             to_enable = (seen_S && parser.value_bool());
+    if (seen_S)
+      if(to_enable)
+       bltouch.bltouch_high_speed = true;
+      else
+        bltouch.bltouch_high_speed = false;
+  #endif
   probe.deploy();
   TERN_(PROBE_TARE, probe.tare());
   report_current_position();
diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h
index c9413d3cea..8b8073b5bb 100644
--- a/Marlin/src/lcd/language/language_en.h
+++ b/Marlin/src/lcd/language/language_en.h
@@ -491,6 +491,7 @@ namespace Language_en {
   LSTR MSG_BLTOUCH_STOW                   = _UxGT("Stow");
   LSTR MSG_BLTOUCH_DEPLOY                 = _UxGT("Deploy");
   LSTR MSG_BLTOUCH_SW_MODE                = _UxGT("SW-Mode");
+  LSTR MSG_BLTOUCH_SPEED_MODE             = _UxGT("High Speed");
   LSTR MSG_BLTOUCH_5V_MODE                = _UxGT("5V-Mode");
   LSTR MSG_BLTOUCH_OD_MODE                = _UxGT("OD-Mode");
   LSTR MSG_BLTOUCH_MODE_STORE             = _UxGT("Mode-Store");
diff --git a/Marlin/src/lcd/menu/menu_bed_corners.cpp b/Marlin/src/lcd/menu/menu_bed_corners.cpp
index 178d466478..8dd737a805 100644
--- a/Marlin/src/lcd/menu/menu_bed_corners.cpp
+++ b/Marlin/src/lcd/menu/menu_bed_corners.cpp
@@ -221,13 +221,13 @@ static void _lcd_level_bed_corners_get_next_position() {
 
   bool _lcd_level_bed_corners_probe(bool verify=false) {
     if (verify) do_blocking_move_to_z(current_position.z + LEVEL_CORNERS_Z_HOP); // do clearance if needed
-    TERN_(BLTOUCH_SLOW_MODE, bltouch.deploy()); // Deploy in LOW SPEED MODE on every probe action
+    if(!bltouch.bltouch_high_speed) bltouch.deploy(); // Deploy in LOW SPEED MODE on every probe action
     do_blocking_move_to_z(last_z - LEVEL_CORNERS_PROBE_TOLERANCE, MMM_TO_MMS(Z_PROBE_FEEDRATE_SLOW)); // Move down to lower tolerance
     if (TEST(endstops.trigger_state(), Z_MIN_PROBE)) { // check if probe triggered
       endstops.hit_on_purpose();
       set_current_from_steppers_for_axis(Z_AXIS);
       sync_plan_position();
-      TERN_(BLTOUCH_SLOW_MODE, bltouch.stow()); // Stow in LOW SPEED MODE on every trigger
+      if(!bltouch.bltouch_high_speed) bltouch.stow(); // Stow in LOW SPEED MODE on every trigger
       // Triggered outside tolerance range?
       if (ABS(current_position.z - last_z) > LEVEL_CORNERS_PROBE_TOLERANCE) {
         last_z = current_position.z; // Above tolerance. Set a new Z for subsequent corners.
@@ -253,7 +253,7 @@ static void _lcd_level_bed_corners_get_next_position() {
       }
       idle();
     }
-    TERN_(BLTOUCH_SLOW_MODE, bltouch.stow());
+    if(!bltouch.bltouch_high_speed) bltouch.stow();
     ui.goto_screen(_lcd_draw_probing);
     return (probe_triggered);
   }
@@ -267,13 +267,14 @@ static void _lcd_level_bed_corners_get_next_position() {
     do {
       ui.refresh(LCDVIEW_REDRAW_NOW);
       _lcd_draw_probing();                                // update screen with # of good points
-      do_blocking_move_to_z(SUM_TERN(BLTOUCH_HS_MODE, current_position.z + LEVEL_CORNERS_Z_HOP, 7)); // clearance
+      float z_raise_probe = Z_CLEARANCE_BETWEEN_PROBES if(bltouch.bltouch_high_speed) + 7;
+      do_blocking_move_to_z(SUM_TERN(z_raise_probe, current_position.z + LEVEL_CORNERS_Z_HOP, 7)); // clearance
 
       _lcd_level_bed_corners_get_next_position();         // Select next corner coordinates
       current_position -= probe.offset_xy;                // Account for probe offsets
       do_blocking_move_to_xy(current_position);           // Goto corner
 
-      TERN_(BLTOUCH_HS_MODE, bltouch.deploy());           // Deploy in HIGH SPEED MODE
+      if(bltouch.bltouch_high_speed) bltouch.deploy();           // Deploy in HIGH SPEED MODE
       if (!_lcd_level_bed_corners_probe()) {              // Probe down to tolerance
         if (_lcd_level_bed_corners_raise()) {             // Prompt user to raise bed if needed
           #if ENABLED(LEVEL_CORNERS_VERIFY_RAISED)        // Verify
@@ -294,11 +295,11 @@ static void _lcd_level_bed_corners_get_next_position() {
 
     } while (good_points < nr_edge_points); // loop until all points within tolerance
 
-    #if ENABLED(BLTOUCH_HS_MODE)
+    if (bltouch.bltouch_high_speed)
       // In HIGH SPEED MODE do clearance and stow at the very end
       do_blocking_move_to_z(current_position.z + LEVEL_CORNERS_Z_HOP);
       bltouch.stow();
-    #endif
+    }
 
     ui.goto_screen(_lcd_draw_level_prompt); // prompt for bed leveling
     ui.set_selection(true);
diff --git a/Marlin/src/lcd/menu/menu_configuration.cpp b/Marlin/src/lcd/menu/menu_configuration.cpp
index 0e11cd211a..5865d4898c 100644
--- a/Marlin/src/lcd/menu/menu_configuration.cpp
+++ b/Marlin/src/lcd/menu/menu_configuration.cpp
@@ -235,6 +235,7 @@ void menu_advanced_settings();
     ACTION_ITEM(MSG_BLTOUCH_DEPLOY, bltouch._deploy);
     ACTION_ITEM(MSG_BLTOUCH_STOW, bltouch._stow);
     ACTION_ITEM(MSG_BLTOUCH_SW_MODE, bltouch._set_SW_mode);
+    EDIT_ITEM(bool, MSG_BLTOUCH_SPEED_MODE, &bltouch.bltouch_high_speed);
     #if ENABLED(BLTOUCH_LCD_VOLTAGE_MENU)
       CONFIRM_ITEM(MSG_BLTOUCH_5V_MODE, MSG_BLTOUCH_5V_MODE, MSG_BUTTON_CANCEL, bltouch._set_5V_mode, nullptr, GET_TEXT(MSG_BLTOUCH_MODE_CHANGE));
       CONFIRM_ITEM(MSG_BLTOUCH_OD_MODE, MSG_BLTOUCH_OD_MODE, MSG_BUTTON_CANCEL, bltouch._set_OD_mode, nullptr, GET_TEXT(MSG_BLTOUCH_MODE_CHANGE));
diff --git a/Marlin/src/lcd/menu/menu_tramming.cpp b/Marlin/src/lcd/menu/menu_tramming.cpp
index 5cdcf75c1e..5109a699ed 100644
--- a/Marlin/src/lcd/menu/menu_tramming.cpp
+++ b/Marlin/src/lcd/menu/menu_tramming.cpp
@@ -51,7 +51,8 @@ static int8_t reference_index; // = 0
 static bool probe_single_point() {
   do_blocking_move_to_z(TERN(BLTOUCH, Z_CLEARANCE_DEPLOY_PROBE, Z_CLEARANCE_BETWEEN_PROBES));
   // Stow after each point with BLTouch "HIGH SPEED" mode for push-pin safety
-  const float z_probed_height = probe.probe_at_point(tramming_points[tram_index], TERN(BLTOUCH_HS_MODE, PROBE_PT_STOW, PROBE_PT_RAISE), 0, true);
+
+  const float z_probed_height = probe.probe_at_point(tramming_points[tram_index], if(bltouch.bltouch_high_speed) PROBE_PT_STOW else PROBE_PT_RAISE), 0, true);
   z_measured[tram_index] = z_probed_height;
   if (reference_index < 0) reference_index = tram_index;
   move_to_tramming_wait_pos();
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 1f13bd22ee..091facc94c 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -1799,8 +1799,8 @@ void prepare_line_to_destination() {
     if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Home Fast: ", move_length, "mm");
     do_homing_move(axis, move_length, 0.0, !use_probe_bump);
 
-    #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
-      if (axis == Z_AXIS) bltouch.stow(); // Intermediate STOW (in LOW SPEED MODE)
+    #if ENABLED(HOMING_Z_WITH_PROBE, BLTOUCH)
+      if (axis == Z_AXIS && !bltouch.bltouch_high_speed) bltouch.stow(); // Intermediate STOW (in LOW SPEED MODE)
     #endif
 
     // If a second homing move is configured...
@@ -1833,8 +1833,9 @@ void prepare_line_to_destination() {
         }
       #endif
 
-      #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH_SLOW_MODE)
-        if (axis == Z_AXIS && bltouch.deploy()) return; // Intermediate DEPLOY (in LOW SPEED MODE)
+      #if BOTH(HOMING_Z_WITH_PROBE, BLTOUCH)
+        if(!bltouch.bltouch_high_speed)
+          if (axis == Z_AXIS && bltouch.deploy()) return; // Intermediate DEPLOY (in LOW SPEED MODE)
       #endif
 
       // Slow move towards endstop until triggered
diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp
index 4078effe76..f2f5eee520 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -487,8 +487,10 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
   #if BOTH(HAS_TEMP_HOTEND, WAIT_FOR_HOTEND)
     thermalManager.wait_for_hotend_heating(active_extruder);
   #endif
-
-  if (TERN0(BLTOUCH_SLOW_MODE, bltouch.deploy())) return true; // Deploy in LOW SPEED MODE on every probe action
+  #if ENABLED(BLTOUCH)
+    if(!bltouch.bltouch_high_speed)
+      if (bltouch.deploy()) return true; // Deploy in LOW SPEED MODE on every probe action
+  #endif
 
   // Disable stealthChop if used. Enable diag1 pin on driver.
   #if ENABLED(SENSORLESS_PROBING)
@@ -528,10 +530,11 @@ bool Probe::probe_down_to_z(const_float_t z, const_feedRate_t fr_mm_s) {
     if (probe.test_sensitivity.z) tmc_disable_stallguard(stepperZ, stealth_states.z);
     set_homing_current(false);
   #endif
-
-  if (probe_triggered && TERN0(BLTOUCH_SLOW_MODE, bltouch.stow())) // Stow in LOW SPEED MODE on every trigger
-    return true;
-
+  #if ENABLED(BLTOUCH)
+    if(!bltouch.bltouch_high_speed)
+      if (probe_triggered && bltouch.stow()) // Stow in LOW SPEED MODE on every trigger
+        return true;
+  #endif
   // Clear endstop flags
   endstops.hit_on_purpose();
 
@@ -760,8 +763,8 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai
     DEBUG_POS("", current_position);
   }
 
-  #if BOTH(BLTOUCH, BLTOUCH_HS_MODE)
-    if (bltouch.triggered()) bltouch._reset();
+  #if ENABLED(BLTOUCH)
+    if (bltouch.triggered() && bltouch.bltouch_high_speed) bltouch._reset();
   #endif
 
   // On delta keep Z below clip height or do_blocking_move_to will abort
diff --git a/Marlin/src/module/settings.cpp b/Marlin/src/module/settings.cpp
index 6b7143e82a..9698348ae3 100644
--- a/Marlin/src/module/settings.cpp
+++ b/Marlin/src/module/settings.cpp
@@ -36,7 +36,7 @@
  */
 
 // Change EEPROM version if the structure changes
-#define EEPROM_VERSION "V85"
+#define EEPROM_VERSION "V86"
 #define EEPROM_OFFSET 100
 
 // Check the integrity of data offsets.
@@ -277,6 +277,7 @@ typedef struct SettingsDataStruct {
   // BLTOUCH
   //
   bool bltouch_last_written_mode;
+  bool bltouch_high_speed;.
 
   //
   // Kinematic Settings
@@ -861,6 +862,10 @@ void MarlinSettings::postprocess() {
       _FIELD_TEST(bltouch_last_written_mode);
       const bool bltouch_last_written_mode = TERN(BLTOUCH, bltouch.last_written_mode, false);
       EEPROM_WRITE(bltouch_last_written_mode);
+
+      _FIELD_TEST(bltouch_high_speed);
+      const bool bltouch_high_speed = TERN(BLTOUCH, bltouch.bltouch_high_speed, false);
+      EEPROM_WRITE(bltouch_high_speed);
     }
 
     //
@@ -1732,6 +1737,14 @@ void MarlinSettings::postprocess() {
           bool bltouch_last_written_mode;
         #endif
         EEPROM_READ(bltouch_last_written_mode);
+
+        _FIELD_TEST(bltouch_high_speed);
+        #if ENABLED(BLTOUCH)
+          const bool &bltouch_high_speed = bltouch.bltouch_high_speed;
+        #else
+          bool bltouch_high_speed;
+        #endif
+        EEPROM_READ(bltouch_last_written_mode);
       }
 
       //
@@ -2729,12 +2742,9 @@ void MarlinSettings::reset() {
   TERN_(EDITABLE_SERVO_ANGLES, COPY(servo_angles, base_servo_angles)); // When not editable only one copy of servo angles exists
 
   //
-  // BLTOUCH
+  // BLTouch
   //
-  //#if ENABLED(BLTOUCH)
-  //  bltouch.last_written_mode;
-  //#endif
-
+  TERN_(BLTOUCH_HS_MODE, bltouch.bltouch_high_speed = true);
   //
   // Kinematic settings
   //