G34 Mechanical Gantry Calibration (like Prusa M915) (#18972)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>

Marlin/Configuration_adv.h

 #endif
 
 /**
+ * Mechanical Gantry Calibration
+ * Modern replacement for the Prusa TMC_Z_CALIBRATION.
+ * Adds capability to work with any adjustable current drivers.
+ * Implemented as G34 because M915 is deprecated.
+ */
+//#define MECHANICAL_GANTRY_CALIBRATION
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+  #define GANTRY_CALIBRATION_CURRENT          600     // Default calibration current in ma
+  #define GANTRY_CALIBRATION_EXTRA_HEIGHT      15     // Extra distance in mm past Z_###_POS to move
+  #define GANTRY_CALIBRATION_FEEDRATE         500     // Feedrate for correction move
+  //#define GANTRY_CALIBRATION_TO_MIN                 // Enable to calibrate Z in the MIN direction
+
+  //#define GANTRY_CALIBRATION_SAFE_POSITION  { X_CENTER, Y_CENTER } // Safe position for nozzle
+  //#define GANTRY_CALIBRATION_XY_PARK_FEEDRATE 3000  // XY Park Feedrate - MMM
+  //#define GANTRY_CALIBRATION_COMMANDS_PRE   ""
+  #define GANTRY_CALIBRATION_COMMANDS_POST  "G28"     // G28 highly recommended to ensure an accurate position
+#endif
+
+/**
  * MAX7219 Debug Matrix
  *
  * Add support for a low-cost 8x8 LED Matrix based on the Max7219 chip as a realtime status display.

Marlin/src/gcode/calibrate/G34.cpp

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../inc/MarlinConfigPre.h"
+
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+
+#include "../gcode.h"
+#include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/endstops.h"
+
+#if HAS_LEVELING
+  #include "../../feature/bedlevel/bedlevel.h"
+#endif
+
+#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
+#include "../../core/debug_out.h"
+
+void GcodeSuite::G34() {
+
+  if (homing_needed()) return;
+
+  TEMPORARY_SOFT_ENDSTOP_STATE(false);
+  TEMPORARY_BED_LEVELING_STATE(false);
+  TemporaryGlobalEndstopsState unlock_z(false);
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_PRE
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_PRE));
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Sub Commands Processed");
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_SAFE_POSITION
+    // Move XY to safe position
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Parking XY");
+    const xy_pos_t safe_pos = GANTRY_CALIBRATION_SAFE_POSITION;
+    do_blocking_move_to(safe_pos, MMM_TO_MMS(GANTRY_CALIBRATION_XY_PARK_FEEDRATE));
+  #endif
+
+  const float move_distance = parser.intval('Z', GANTRY_CALIBRATION_EXTRA_HEIGHT),
+              zbase = ENABLED(GANTRY_CALIBRATION_TO_MIN) ? Z_MIN_POS : Z_MAX_POS,
+              zpounce = zbase - move_distance, zgrind = zbase + move_distance;
+
+  // Move Z to pounce position
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Setting Z Pounce");
+  do_blocking_move_to_z(zpounce, MMM_TO_MMS(HOMING_FEEDRATE_Z));
+
+  // Store current motor settings, then apply reduced value
+
+  #define _REDUCE_CURRENT ANY(HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_PWM, HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_I2C, HAS_TRINAMIC_CONFIG)
+  #if _REDUCE_CURRENT
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Reducing Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(Z_AXIS, target_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    const uint32_t previous_current = stepper.motor_current_setting[Z_AXIS];
+    stepper.set_digipot_current(1, target_current);
+  #elif HAS_MOTOR_CURRENT_DAC
+    const float target_current = parser.floatval('S', GANTRY_CALIBRATION_CURRENT);
+    const float previous_current = dac_amps(Z_AXIS, target_current);
+    stepper_dac.set_current_value(Z_AXIS, target_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    previous_current = dac_amps(Z_AXIS);
+    digipot_i2c.set_current(Z_AXIS, target_current)
+  #elif HAS_TRINAMIC_CONFIG
+    const uint16_t target_current = parser.intval('S', GANTRY_CALIBRATION_CURRENT);
+    static uint16_t previous_current_arr[NUM_Z_STEPPER_DRIVERS];
+    #if AXIS_IS_TMC(Z)
+      previous_current_arr[0] = stepperZ.getMilliamps();
+      stepperZ.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      previous_current_arr[1] = stepperZ2.getMilliamps();
+      stepperZ2.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      previous_current_arr[2] = stepperZ3.getMilliamps();
+      stepperZ3.rms_current(target_current);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      previous_current_arr[3] = stepperZ4.getMilliamps();
+      stepperZ4.rms_current(target_current);
+    #endif
+  #endif
+
+  // Do Final Z move to adjust
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Final Z Move");
+  do_blocking_move_to_z(zgrind, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  // Back off end plate, back to normal motion range
+  if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Z Backoff");
+  do_blocking_move_to_z(zpounce, MMM_TO_MMS(GANTRY_CALIBRATION_FEEDRATE));
+
+  #if _REDUCE_CURRENT
+    // Reset current to original values
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Restore Current");
+  #endif
+
+  #if HAS_MOTOR_CURRENT_SPI
+    stepper.set_digipot_current(Z_AXIS, previous_current);
+  #elif HAS_MOTOR_CURRENT_PWM
+    stepper.set_digipot_current(1, previous_current);
+  #elif HAS_MOTOR_CURRENT_DAC
+    stepper_dac.set_current_value(Z_AXIS, previous_current);
+  #elif ENABLED(HAS_MOTOR_CURRENT_I2C)
+    digipot_i2c.set_current(Z_AXIS, previous_current)
+  #elif HAS_TRINAMIC_CONFIG
+    #if AXIS_IS_TMC(Z)
+      stepperZ.rms_current(previous_current_arr[0]);
+    #endif
+    #if AXIS_IS_TMC(Z2)
+      stepperZ2.rms_current(previous_current_arr[1]);
+    #endif
+    #if AXIS_IS_TMC(Z3)
+      stepperZ3.rms_current(previous_current_arr[2]);
+    #endif
+    #if AXIS_IS_TMC(Z4)
+      stepperZ4.rms_current(previous_current_arr[3]);
+    #endif
+  #endif
+
+  #ifdef GANTRY_CALIBRATION_COMMANDS_POST
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Running Post Commands");
+    gcode.process_subcommands_now_P(PSTR(GANTRY_CALIBRATION_COMMANDS_POST));
+  #endif
+}
+
+#endif // MECHANICAL_GANTRY_CALIBRATION

Marlin/src/gcode/calibrate/G34_M422.cpp

  *
  */
 
-#include "../../inc/MarlinConfig.h"
+#include "../../inc/MarlinConfigPre.h"
 
 #if ENABLED(Z_STEPPER_AUTO_ALIGN)
 
 #include "../../feature/z_stepper_align.h"
 
 #include "../gcode.h"
-#include "../../module/planner.h"
-#include "../../module/stepper.h"
 #include "../../module/motion.h"
+#include "../../module/stepper.h"
+#include "../../module/planner.h"
 #include "../../module/probe.h"
-
-#if HAS_MULTI_HOTEND
-  #include "../../module/tool_change.h"
-#endif
+#include "../../lcd/ultralcd.h" // for LCD_MESSAGEPGM
 
 #if HAS_LEVELING
   #include "../../feature/bedlevel/bedlevel.h"
 #endif
 
+#if HAS_MULTI_HOTEND
+  #include "../../module/tool_change.h"
+#endif
+
 #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
-   #include "../../libs/least_squares_fit.h"
+  #include "../../libs/least_squares_fit.h"
 #endif
 
 #define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
     // 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 * BOTH(BLTOUCH, BLTOUCH_HS_MODE))
 
     // Compute a worst-case clearance height to probe from. After the first
     // iteration this will be re-calculated based on the actual bed position
           z_maxdiff = 0.0f,
           amplification = z_auto_align_amplification;
 
-    // These are needed after the for-loop
-    uint8_t iteration;
-    bool err_break = false;
-    float z_measured_min;
-
     #if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
       bool adjustment_reverse = false;
     #endif
 
-    // 'iteration' is declared above and is also used after the for-loop.
-    // *not* the same as LOOP_L_N(iteration, z_auto_align_iterations)
-    for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) {
+    #if HAS_DISPLAY
+      PGM_P const msg_iteration = GET_TEXT(MSG_ITERATION);
+      const uint8_t iter_str_len = strlen_P(msg_iteration);
+    #endif
+
+    // Final z and iteration values will be used after breaking the loop
+    float z_measured_min;
+    uint8_t iteration = 0;
+    bool err_break = false; // To break out of nested loops
+    while (iteration < z_auto_align_iterations) {
       if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
 
-      SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1));
+      const int iter = iteration + 1;
+      SERIAL_ECHOLNPAIR("\nG34 Iteration: ", iter);
+      #if HAS_DISPLAY
+        char str[iter_str_len + 2 + 1];
+        sprintf_P(str, msg_iteration, iter);
+        ui.set_status(str);
+      #endif
 
       // Initialize minimum value
       z_measured_min =  100000.0f;
         // current_position.z has been manually altered in the "dirty trick" above.
         const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true, false);
         if (isnan(z_probed_height)) {
-          SERIAL_ECHOLNPGM("Probing failed.");
+          SERIAL_ECHOLNPGM("Probing failed");
+          LCD_MESSAGEPGM(MSG_LCD_PROBING_FAILED);
           err_break = true;
           break;
         }
           , " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
         #endif
       );
+      #if HAS_DISPLAY
+        char fstr1[10];
+        #if NUM_Z_STEPPER_DRIVERS == 2
+          char msg[6 + (6 + 5) * 1 + 1];
+        #else
+          char msg[6 + (6 + 5) * 3 + 1], fstr2[10], fstr3[10];
+        #endif
+        sprintf_P(msg,
+          PSTR("Diffs Z1-Z2=%s"
+            #if NUM_Z_STEPPER_DRIVERS == 3
+              " Z2-Z3=%s"
+              " Z3-Z1=%s"
+            #endif
+          ), dtostrf(ABS(z_measured[0] - z_measured[1]), 1, 3, fstr1)
+          #if NUM_Z_STEPPER_DRIVERS == 3
+            , dtostrf(ABS(z_measured[1] - z_measured[2]), 1, 3, fstr2)
+            , dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)
+          #endif
+        );
+        ui.set_status(msg);
+      #endif
+
+      auto decreasing_accuracy = [](const float &v1, const float &v2){
+        if (v1 < v2 * 0.7f) {
+          SERIAL_ECHOLNPGM("Decreasing Accuracy Detected.");
+          LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY);
+          return true;
+        }
+        return false;
+      };
 
       #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
+
         // Check if the applied corrections go in the correct direction.
         // Calculate the sum of the absolute deviations from the mean of the probe measurements.
         // Compare to the last iteration to ensure it's getting better.
           z_align_level_indicator += ABS(z_measured[zstepper] - z_measured_mean);
 
         // If it's getting worse, stop and throw an error
-        if (last_z_align_level_indicator < z_align_level_indicator * 0.7f) {
-          SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
-          err_break = true;
-          break;
-        }
+        err_break = decreasing_accuracy(last_z_align_level_indicator, z_align_level_indicator);
+        if (err_break) break;
 
         last_z_align_level_indicator = z_align_level_indicator;
       #endif
           if (z_align_abs) amplification = (iteration == 1) ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification;
 
           // Check for less accuracy compared to last move
-          if (last_z_align_move[zstepper] < z_align_abs * 0.7f) {
-            SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
+          if (decreasing_accuracy(last_z_align_move[zstepper], z_align_abs)) {
             if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " last_z_align_move = ", last_z_align_move[zstepper]);
             if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " z_align_abs = ", z_align_abs);
             adjustment_reverse = !adjustment_reverse;
 
       if (err_break) break;
 
-      if (success_break) { SERIAL_ECHOLNPGM("Target accuracy achieved."); break; }
+      if (success_break) {
+        SERIAL_ECHOLNPGM("Target accuracy achieved.");
+        LCD_MESSAGEPGM(MSG_ACCURACY_ACHIEVED);
+        break;
+      }
 
-    } // for (iteration)
+      iteration++;
+    } // while (iteration < z_auto_align_iterations)
 
     if (err_break)
       SERIAL_ECHOLNPGM("G34 aborted.");

Marlin/src/gcode/gcode.cpp

         case 33: G33(); break;                                    // G33: Delta Auto-Calibration
       #endif
 
-      #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+      #if EITHER(Z_STEPPER_AUTO_ALIGN, MECHANICAL_GANTRY_CALIBRATION)
         case 34: G34(); break;                                    // G34: Z Stepper automatic alignment using probe
       #endif
 

Marlin/src/gcode/gcode.h

 
   TERN_(DELTA_AUTO_CALIBRATION, static void G33());
 
-  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+  #if EITHER(Z_STEPPER_AUTO_ALIGN, MECHANICAL_GANTRY_CALIBRATION)
     static void G34();
-    static void M422();
   #endif
 
+  TERN_(Z_STEPPER_AUTO_ALIGN, static void M422());
+
   TERN_(ASSISTED_TRAMMING, static void G35());
 
   TERN_(G38_PROBE_TARGET, static void G38(const int8_t subcode));

Marlin/src/inc/SanityCheck.h

 #elif defined(CHAMBER_HEATER_PIN)
   #error "CHAMBER_HEATER_PIN is now HEATER_CHAMBER_PIN. Please update your configuration and/or pins."
 #elif defined(TMC_Z_CALIBRATION)
-  #error "TMC_Z_CALIBRATION has been deprecated in favor of Z_STEPPER_AUTO_ALIGN. Please update your configuration."
+  #error "TMC_Z_CALIBRATION has been deprecated in favor of MECHANICAL_GANTRY_CALIBRATION. Please update your configuration."
 #elif defined(Z_MIN_PROBE_ENDSTOP)
   #error "Z_MIN_PROBE_ENDSTOP is no longer required. Please remove it from Configuration.h."
 #elif defined(DUAL_NOZZLE_DUPLICATION_MODE)
   #endif
 #endif
 
+#if ENABLED(MECHANICAL_GANTRY_CALIBRATION)
+  #if NONE(HAS_MOTOR_CURRENT_DAC, HAS_MOTOR_CURRENT_SPI, HAS_MOTOR_CURRENT_DAC, HAS_TRINAMIC_CONFIG, HAS_MOTOR_CURRENT_PWM)
+    #error "It is highly recommended to have adjustable current drivers to prevent damage. Disable this line to continue anyway."
+  #elif !defined(GANTRY_CALIBRATION_CURRENT)
+    #error "MECHANICAL_GANTRY_CALIBRATION Requires GANTRY_CALIBRATION_CURRENT to be set."
+  #elif !defined(GANTRY_CALIBRATION_EXTRA_HEIGHT)
+    #error "MECHANICAL_GANTRY_CALIBRATION Requires GANTRY_CALIBRATION_EXTRA_HEIGHT to be set."
+  #elif !defined(GANTRY_CALIBRATION_FEEDRATE)
+    #error "MECHANICAL_GANTRY_CALIBRATION Requires GANTRY_CALIBRATION_FEEDRATE to be set."
+  #endif
+  #if defined(GANTRY_CALIBRATION_SAFE_POSITION) && !defined(GANTRY_CALIBRATION_XY_PARK_FEEDRATE)
+    #error "GANTRY_CALIBRATION_SAFE_POSITION Requires GANTRY_CALIBRATION_XY_PARK_FEEDRATE to be set."
+  #endif
+#endif
+
 #if ENABLED(PRINTCOUNTER) && DISABLED(EEPROM_SETTINGS)
   #error "PRINTCOUNTER requires EEPROM_SETTINGS. Please update your Configuration."
 #endif

Marlin/src/lcd/language/language_en.h

   PROGMEM Language_Str MSG_AUTO_HOME_Z                     = _UxGT("Home Z");
   PROGMEM Language_Str MSG_AUTO_Z_ALIGN                    = _UxGT("Auto Z-Align");
   PROGMEM Language_Str MSG_ASSISTED_TRAMMING               = _UxGT("Assisted Tramming");
+  PROGMEM Language_Str MSG_ITERATION                       = _UxGT("G34 Iteration: %i");
+  PROGMEM Language_Str MSG_DECREASING_ACCURACY             = _UxGT("Accuracy Decreasing!");
+  PROGMEM Language_Str MSG_ACCURACY_ACHIEVED               = _UxGT("Accuracy Achieved");
   PROGMEM Language_Str MSG_LEVEL_BED_HOMING                = _UxGT("Homing XYZ");
   PROGMEM Language_Str MSG_LEVEL_BED_WAITING               = _UxGT("Click to Begin");
   PROGMEM Language_Str MSG_LEVEL_BED_NEXT_POINT            = _UxGT("Next Point");

Marlin/src/lcd/menu/menu_motion.cpp

   //
   // Auto Z-Align
   //
-  #if ENABLED(Z_STEPPER_AUTO_ALIGN)
+  #if EITHER(Z_STEPPER_AUTO_ALIGN, MECHANICAL_GANTRY_CALIBRATION)
     GCODES_ITEM(MSG_AUTO_Z_ALIGN, PSTR("G34"));
   #endif
 

buildroot/tests/LPC1769-tests

 opt_set X_DRIVER_TYPE TMC2130
 opt_set Y_DRIVER_TYPE TMC2130
 opt_set Z_DRIVER_TYPE TMC2130
-opt_enable AUTO_BED_LEVELING_BILINEAR EEPROM_SETTINGS EEPROM_CHITCHAT \
+opt_enable AUTO_BED_LEVELING_BILINEAR EEPROM_SETTINGS EEPROM_CHITCHAT MECHANICAL_GANTRY_CALIBRATION \
            TMC_USE_SW_SPI MONITOR_DRIVER_STATUS STEALTHCHOP_XY STEALTHCHOP_Z HYBRID_THRESHOLD \
            SENSORLESS_PROBING Z_SAFE_HOMING X_STALL_SENSITIVITY Y_STALL_SENSITIVITY Z_STALL_SENSITIVITY TMC_DEBUG \
            EXPERIMENTAL_I2CBUS

platformio.ini

   -<src/gcode/bedlevel/G42.cpp>
   -<src/gcode/bedlevel/M420.cpp>
   -<src/gcode/calibrate/G33.cpp>
+  -<src/gcode/calibrate/G34.cpp>
   -<src/gcode/calibrate/G34_M422.cpp>
   -<src/gcode/calibrate/G76_M192_M871.cpp>
   -<src/gcode/calibrate/G425.cpp>
 EXT_SOLENOID|MANUAL_SOLENOID_CONTROL = src_filter=+<src/feature/solenoid.cpp> +<src/gcode/control/M380_M381.cpp>
 HAS_CUTTER              = src_filter=+<src/feature/spindle_laser.cpp> +<src/gcode/control/M3-M5.cpp>
 EXPERIMENTAL_I2CBUS     = src_filter=+<src/feature/twibus.cpp> +<src/gcode/feature/i2c>
+MECHANICAL_GANTRY_CAL.+ = src_filter=+<src/gcode/calibrate/G34.cpp>
 Z_STEPPER_AUTO_ALIGN    = src_filter=+<src/feature/z_stepper_align.cpp> +<src/gcode/calibrate/G34_M422.cpp>
 G26_MESH_VALIDATION     = src_filter=+<src/gcode/bedlevel/G26.cpp>
 ASSISTED_TRAMMING       = src_filter=+<src/gcode/bedlevel/G35.cpp>