Fix G76 probe height / position (MarlinFirmware#17392)

HairingX · Jun 16, 2021 · 2c1679b · 2c1679b
1 parent 12e036e
commit 2c1679b
Show file tree

Hide file tree

Showing 6 changed files with 67 additions and 66 deletions.
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
@@ -1600,18 +1600,11 @@
   // Add additional compensation depending on hotend temperature
   // Note: this values cannot be calibrated and have to be set manually
   #if ENABLED(PROBE_TEMP_COMPENSATION)
-    // Max temperature that can be reached by heated bed.
-    // This is required only for the calibration process.
-    #define PTC_MAX_BED_TEMP BED_MAXTEMP
-
     // Park position to wait for probe cooldown
-    #define PTC_PARK_POS_X 0.0F
-    #define PTC_PARK_POS_Y 0.0F
-    #define PTC_PARK_POS_Z 100.0F
+    #define PTC_PARK_POS   { 0, 0, 100 }
 
     // Probe position to probe and wait for probe to reach target temperature
-    #define PTC_PROBE_POS_X  90.0F
-    #define PTC_PROBE_POS_Y 100.0F
+    #define PTC_PROBE_POS  { 90, 100 }
 
     // Enable additional compensation using hotend temperature
     // Note: this values cannot be calibrated automatically but have to be set manually

diff --git a/Marlin/src/feature/probe_temp_comp.cpp b/Marlin/src/feature/probe_temp_comp.cpp
@@ -29,11 +29,11 @@
 
 ProbeTempComp temp_comp;
 
-int16_t ProbeTempComp::z_offsets_probe[ProbeTempComp::cali_info_init[TSI_PROBE].measurements],  // = {0}
-        ProbeTempComp::z_offsets_bed[ProbeTempComp::cali_info_init[TSI_BED].measurements];      // = {0}
+int16_t ProbeTempComp::z_offsets_probe[cali_info_init[TSI_PROBE].measurements],  // = {0}
+        ProbeTempComp::z_offsets_bed[cali_info_init[TSI_BED].measurements];      // = {0}
 
 #if ENABLED(USE_TEMP_EXT_COMPENSATION)
-  int16_t ProbeTempComp::z_offsets_ext[ProbeTempComp::cali_info_init[TSI_EXT].measurements];    // = {0}
+  int16_t ProbeTempComp::z_offsets_ext[cali_info_init[TSI_EXT].measurements];    // = {0}
 #endif
 
 int16_t *ProbeTempComp::sensor_z_offsets[TSI_COUNT] = {
@@ -44,9 +44,9 @@ int16_t *ProbeTempComp::sensor_z_offsets[TSI_COUNT] = {
 };
 
 const temp_calib_t ProbeTempComp::cali_info[TSI_COUNT] = {
-  ProbeTempComp::cali_info_init[TSI_PROBE], ProbeTempComp::cali_info_init[TSI_BED]
+  cali_info_init[TSI_PROBE], cali_info_init[TSI_BED]
   #if ENABLED(USE_TEMP_EXT_COMPENSATION)
-    , ProbeTempComp::cali_info_init[TSI_EXT]
+    , cali_info_init[TSI_EXT]
   #endif
 };
 

diff --git a/Marlin/src/feature/probe_temp_comp.h b/Marlin/src/feature/probe_temp_comp.h
@@ -44,30 +44,28 @@ typedef struct {
  * Z-probes like the P.I.N.D.A V2 allow for compensation of
  * measurement errors/shifts due to changed temperature.
  */
+
+static constexpr temp_calib_t cali_info_init[TSI_COUNT] = {
+    {  10,  5,  30,  30 + 10 *  5 },       // Probe
+    {  10,  5,  60,  60 + 10 *  5 },       // Bed
+  #if ENABLED(USE_TEMP_EXT_COMPENSATION)
+    {  20,  5, 180, 180 +  5 * 20 }        // Extruder
+  #endif
+};
+
 class ProbeTempComp {
   public:
 
-    static constexpr temp_calib_t cali_info_init[TSI_COUNT] = {
-        {  10,  5,  30,  30 + 10 *  5 },       // Probe
-        {  10,  5,  60,  60 + 10 *  5 },       // Bed
-      #if ENABLED(USE_TEMP_EXT_COMPENSATION)
-        {  20,  5, 180, 180 +  5 * 20 }        // Extruder
-      #endif
-    };
     static const temp_calib_t cali_info[TSI_COUNT];
 
     // Where to park nozzle to wait for probe cooldown
-    static constexpr float park_point_x = PTC_PARK_POS_X,
-                           park_point_y = PTC_PARK_POS_Y,
-                           park_point_z = PTC_PARK_POS_Z,
-                           // XY coordinates of nozzle for probing the bed
-                           measure_point_x     = PTC_PROBE_POS_X,   // Coordinates to probe
-                           measure_point_y     = PTC_PROBE_POS_Y;
-                           //measure_point_x     = 12.0f,           // Coordinates to probe on MK52 magnetic heatbed
-                           //measure_point_y     =  7.3f;
-
-    static constexpr int  max_bed_temp         = PTC_MAX_BED_TEMP,  // Max temperature to avoid heating errors
-                          probe_calib_bed_temp = max_bed_temp,      // Bed temperature while calibrating probe
+    static constexpr xyz_pos_t park_point = PTC_PARK_POS;
+
+    // XY coordinates of nozzle for probing the bed
+    static constexpr xy_pos_t measure_point    = PTC_PROBE_POS;     // Coordinates to probe
+                            //measure_point    = { 12.0f, 7.3f };   // Coordinates for the MK52 magnetic heatbed
+
+    static constexpr int  probe_calib_bed_temp = BED_MAXTEMP - 10,  // Bed temperature while calibrating probe
                           bed_calib_probe_temp = 30;                // Probe temperature while calibrating bed
 
     static int16_t *sensor_z_offsets[TSI_COUNT],

diff --git a/Marlin/src/gcode/calibrate/G76_M871.cpp b/Marlin/src/gcode/calibrate/G76_M871.cpp
@@ -103,13 +103,19 @@ void GcodeSuite::G76() {
     return false;
   };
 
-  auto g76_probe = [](const xy_pos_t &xypos) {
+  auto g76_probe = [](const TempSensorID sid, uint16_t &targ, const xy_pos_t &nozpos) {
     do_blocking_move_to_z(5.0); // Raise nozzle before probing
-    const float measured_z = probe.probe_at_point(xypos, PROBE_PT_NONE, 0, false);  // verbose=0, probe_relative=false
+    const float measured_z = probe.probe_at_point(nozpos, PROBE_PT_NONE, 0, false);  // verbose=0, probe_relative=false
     if (isnan(measured_z))
       SERIAL_ECHOLNPGM("!Received NAN. Aborting.");
-    else
+    else {
       SERIAL_ECHOLNPAIR_F("Measured: ", measured_z);
+      if (targ == cali_info_init[sid].start_temp)
+        temp_comp.prepare_new_calibration(measured_z);
+      else
+        temp_comp.push_back_new_measurement(sid, measured_z);
+      targ += cali_info_init[sid].temp_res;
+    }
     return measured_z;
   };
 
@@ -125,8 +131,9 @@ void GcodeSuite::G76() {
   // Synchronize with planner
   planner.synchronize();
 
-  const xyz_pos_t parkpos = { temp_comp.park_point_x, temp_comp.park_point_y, temp_comp.park_point_z };
-  const xy_pos_t ppos = { temp_comp.measure_point_x, temp_comp.measure_point_y };
+  const xyz_pos_t parkpos = temp_comp.park_point,
+            probe_pos_xyz = temp_comp.measure_point + xyz_pos_t({ 0.0f, 0.0f, 0.5f }),
+              noz_pos_xyz = probe_pos_xyz - probe.offset_xy; // Nozzle position based on probe position
 
   if (do_bed_cal || do_probe_cal) {
     // Ensure park position is reachable
@@ -135,7 +142,7 @@ void GcodeSuite::G76() {
       SERIAL_ECHOLNPGM("!Park");
     else {
       // Ensure probe position is reachable
-      reachable = probe.can_reach(ppos);
+      reachable = probe.can_reach(probe_pos_xyz);
       if (!reachable) SERIAL_ECHOLNPGM("!Probe");
     }
 
@@ -149,8 +156,6 @@ void GcodeSuite::G76() {
 
   remember_feedrate_scaling_off();
 
-  // Nozzle position based on probe position
-  const xy_pos_t noz_pos = ppos - probe.offset_xy;
 
   /******************************************
    * Calibrate bed temperature offsets
@@ -159,9 +164,13 @@ void GcodeSuite::G76() {
   // Report temperatures every second and handle heating timeouts
   millis_t next_temp_report = millis() + 1000;
 
+  auto report_targets = [&](const uint16_t tb, const uint16_t tp) {
+    SERIAL_ECHOLNPAIR("Target Bed:", tb, " Probe:", tp);
+  };
+
   if (do_bed_cal) {
 
-    uint16_t target_bed = temp_comp.cali_info_init[TSI_BED].start_temp,
+    uint16_t target_bed = cali_info_init[TSI_BED].start_temp,
              target_probe = temp_comp.bed_calib_probe_temp;
 
     SERIAL_ECHOLNPGM("Waiting for cooling.");
@@ -176,7 +185,7 @@ void GcodeSuite::G76() {
     for (;;) {
       thermalManager.setTargetBed(target_bed);
 
-      SERIAL_ECHOLNPAIR("Target Bed:", target_bed, " Probe:", target_probe);
+      report_targets(target_bed, target_probe);
 
       // Park nozzle
       do_blocking_move_to(parkpos);
@@ -188,21 +197,13 @@ void GcodeSuite::G76() {
       }
 
       // Move the nozzle to the probing point and wait for the probe to reach target temp
-      do_blocking_move_to_xy(noz_pos);
+      do_blocking_move_to(noz_pos_xyz);
       SERIAL_ECHOLNPGM("Waiting for probe heating.");
       while (thermalManager.degProbe() < target_probe)
         report_temps(next_temp_report);
 
-      const float measured_z = g76_probe(noz_pos);
-      if (isnan(measured_z)) break;
-
-      if (target_bed == temp_comp.cali_info_init[TSI_BED].start_temp)
-        temp_comp.prepare_new_calibration(measured_z);
-      else
-        temp_comp.push_back_new_measurement(TSI_BED, measured_z);
-
-      target_bed += temp_comp.cali_info_init[TSI_BED].temp_res;
-      if (target_bed > temp_comp.max_bed_temp) break;
+      const float measured_z = g76_probe(TSI_BED, target_bed, noz_pos_xyz);
+      if (isnan(measured_z) || target_bed > BED_MAXTEMP - 10) break;
     }
 
     SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());
@@ -231,7 +232,9 @@ void GcodeSuite::G76() {
     const uint16_t target_bed = temp_comp.probe_calib_bed_temp;
     thermalManager.setTargetBed(target_bed);
 
-    uint16_t target_probe = temp_comp.cali_info_init[TSI_PROBE].start_temp;
+    uint16_t target_probe = cali_info_init[TSI_PROBE].start_temp;
+
+    report_targets(target_bed, target_probe);
 
     // Wait for heatbed to reach target temp and probe to cool below target temp
     wait_for_temps(target_bed, target_probe, next_temp_report);
@@ -244,7 +247,7 @@ void GcodeSuite::G76() {
     bool timeout = false;
     for (;;) {
       // Move probe to probing point and wait for it to reach target temperature
-      do_blocking_move_to_xy(noz_pos);
+      do_blocking_move_to(noz_pos_xyz);
 
       SERIAL_ECHOLNPAIR("Waiting for probe heating. Bed:", target_bed, " Probe:", target_probe);
       const millis_t probe_timeout_ms = millis() + 900UL * 1000UL;
@@ -257,16 +260,8 @@ void GcodeSuite::G76() {
       }
       if (timeout) break;
 
-      const float measured_z = g76_probe(noz_pos);
-      if (isnan(measured_z)) break;
-
-      if (target_probe == temp_comp.cali_info_init[TSI_PROBE].start_temp)
-        temp_comp.prepare_new_calibration(measured_z);
-      else
-        temp_comp.push_back_new_measurement(TSI_PROBE, measured_z);
-
-      target_probe += temp_comp.cali_info_init[TSI_PROBE].temp_res;
-      if (target_probe > temp_comp.cali_info_init[TSI_PROBE].end_temp) break;
+      const float measured_z = g76_probe(TSI_PROBE, target_probe, noz_pos_xyz);
+      if (isnan(measured_z) || target_probe > cali_info_init[TSI_PROBE].end_temp) break;
     }
 
     SERIAL_ECHOLNPAIR("Retrieved measurements: ", temp_comp.get_index());

diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h
@@ -491,6 +491,21 @@
   #error "DUGS_UI_MOVE_DIS_OPTION is spelled DGUS_UI_MOVE_DIS_OPTION. Please update Configuration_adv.h."
 #endif
 
+/**
+ * Probe temp compensation requirements
+ */
+#if ENABLED(PROBE_TEMP_COMPENSATION)
+  #if defined(PTC_PARK_POS_X) || defined(PTC_PARK_POS_Y) || defined(PTC_PARK_POS_Z)
+    #error "PTC_PARK_POS_[XYZ] is now PTC_PARK_POS (array). Please update Configuration_adv.h."
+  #elif !defined(PTC_PARK_POS)
+    #error "PROBE_TEMP_COMPENSATION requires PTC_PARK_POS."
+  #elif defined(PTC_PROBE_POS_X) || defined(PTC_PROBE_POS_Y)
+    #error "PTC_PROBE_POS_[XY] is now PTC_PROBE_POS (array). Please update Configuration_adv.h."
+  #elif !defined(PTC_PROBE_POS)
+    #error "PROBE_TEMP_COMPENSATION requires PTC_PROBE_POS."
+  #endif
+#endif
+
 /**
  * Marlin release, version and default string
  */

diff --git a/buildroot/share/tests/rambo-tests b/buildroot/share/tests/rambo-tests
@@ -50,7 +50,7 @@ opt_set GRID_MAX_POINTS_X 16
 opt_set FANMUX0_PIN 53
 opt_disable USE_WATCHDOG
 opt_enable REPRAP_DISCOUNT_SMART_CONTROLLER LCD_PROGRESS_BAR LCD_PROGRESS_BAR_TEST \
-           FIX_MOUNTED_PROBE Z_SAFE_HOMING CODEPENDENT_XY_HOMING PIDTEMPBED \
+           FIX_MOUNTED_PROBE Z_SAFE_HOMING CODEPENDENT_XY_HOMING PIDTEMPBED PROBE_TEMP_COMPENSATION \
            PROBING_HEATERS_OFF PROBING_FANS_OFF PROBING_STEPPERS_OFF WAIT_FOR_BED_HEATER \
            EEPROM_SETTINGS SDSUPPORT SD_REPRINT_LAST_SELECTED_FILE BINARY_FILE_TRANSFER \
            BLINKM PCA9632 RGB_LED RGB_LED_R_PIN RGB_LED_G_PIN RGB_LED_B_PIN LED_CONTROL_MENU \