🐛 Fix G33, Delta radii, reachable (#22795)

Marlin/src/HAL/LPC1768/fast_pwm.cpp

 
 void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) {
   if (!LPC176x::pin_is_valid(pin)) return;
-  if (LPC176x::pwm_attach_pin(pin)) 
+  if (LPC176x::pwm_attach_pin(pin))
     LPC176x::pwm_write_ratio(pin, invert ? 1.0f - (float)v / v_size : (float)v / v_size);  // map 1-254 onto PWM range
 }
 

Marlin/src/gcode/calibrate/G33.cpp

 
 float lcd_probe_pt(const xy_pos_t &xy);
 
-float dcr;
-
 void ac_home() {
   endstops.enable(true);
   TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(true));
  */
 static float calibration_probe(const xy_pos_t &xy, const bool stow, const bool probe_at_offset) {
   #if HAS_BED_PROBE
-    return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, true, probe_at_offset);
+    return probe.probe_at_point(xy, stow ? PROBE_PT_STOW : PROBE_PT_RAISE, 0, probe_at_offset, false);
   #else
     UNUSED(stow);
     return lcd_probe_pt(xy);
 /**
  *  - Probe a grid
  */
-static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each, const bool probe_at_offset) {
+static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_points, const float dcr, const bool towers_set, const bool stow_after_each, const bool probe_at_offset) {
   const bool _0p_calibration      = probe_points == 0,
              _1p_calibration      = probe_points == 1 || probe_points == -1,
              _4p_calibration      = probe_points == 2,
  *  - formulae for approximative forward kinematics in the end-stop displacement matrix
  *  - definition of the matrix scaling parameters
  */
-static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1]) {
+static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_at_pt_axis[NPP + 1], const float dcr) {
   xyz_pos_t pos{0};
 
   LOOP_CAL_ALL(rad) {
   }
 }
 
-static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1]) {
+static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], float z_pt[NPP + 1], const float dcr) {
   const float r_quot = dcr / delta_radius;
 
   #define ZPP(N,I,A) (((1.0f + r_quot * (N)) / 3.0f) * mm_at_pt_axis[I].A)
   z_pt[_AB] = Zp1(_AB, a) + Zp1(_AB, b) + Zm2(_AB, c);
 }
 
-static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
+static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], const float dcr, abc_float_t delta_e, const float delta_r, abc_float_t delta_t) {
   const float z_center = z_pt[CEN];
   abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1];
 
-  reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis);
+  reverse_kinematics_probe_points(z_pt, diff_mm_at_pt_axis, dcr);
 
   delta_radius += delta_r;
   delta_tower_angle_trim += delta_t;
   recalc_delta_settings();
-  reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis);
+  reverse_kinematics_probe_points(z_pt, new_mm_at_pt_axis, dcr);
 
   LOOP_CAL_ALL(rad) diff_mm_at_pt_axis[rad] -= new_mm_at_pt_axis[rad] + delta_e;
-  forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt);
+  forward_kinematics_probe_points(diff_mm_at_pt_axis, z_pt, dcr);
 
   LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
   z_pt[CEN] = z_center;
   recalc_delta_settings();
 }
 
-static float auto_tune_h() {
+static float auto_tune_h(const float dcr) {
   const float r_quot = dcr / delta_radius;
   return RECIPROCAL(r_quot / (2.0f / 3.0f));  // (2/3)/CR
 }
 
-static float auto_tune_r() {
+static float auto_tune_r(const float dcr) {
   constexpr float diff = 0.01f, delta_r = diff;
   float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
   abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
 
-  calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+  calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t);
   r_fac = -(z_pt[__A] + z_pt[__B] + z_pt[__C] + z_pt[_BC] + z_pt[_CA] + z_pt[_AB]) / 6.0f;
   r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
   return r_fac;
 }
 
-static float auto_tune_a() {
+static float auto_tune_a(const float dcr) {
   constexpr float diff = 0.01f, delta_r = 0.0f;
   float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
   abc_float_t delta_e = { 0.0f }, delta_t = { 0.0f };
   delta_t.reset();
   LOOP_LINEAR_AXES(axis) {
     delta_t[axis] = diff;
-    calc_kinematics_diff_probe_points(z_pt, delta_e, delta_r, delta_t);
+    calc_kinematics_diff_probe_points(z_pt, dcr, delta_e, delta_r, delta_t);
     delta_t[axis] = 0;
     a_fac += z_pt[uint8_t((axis * _4P_STEP) - _7P_STEP + NPP) % NPP + 1] / 6.0f;
     a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 6.0f;
  *      P3       Probe all positions: center, towers and opposite towers. Calibrate all.
  *      P4-P10   Probe all positions at different intermediate locations and average them.
  *
- *   Rn.nn  override default calibration Radius
+ *   Rn.nn  Temporary reduce the probe grid by the specified amount (mm)
  *
  *   T   Don't calibrate tower angle corrections
  *
  *
  *   E   Engage the probe for each point
  *
- *   O   Probe at offset points (this is wrong but it seems to work)
+ *   O   Probe at offsetted probe positions (this is wrong but it seems to work)
  *
  * With SENSORLESS_PROBING:
  *   Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.)
     return;
   }
 
-  const bool probe_at_offset = TERN0(HAS_PROBE_XY_OFFSET, parser.boolval('O')),
+  const bool probe_at_offset = TERN0(HAS_PROBE_XY_OFFSET, parser.seen_test('O')),
                   towers_set = !parser.seen_test('T');
 
-  float max_dcr = dcr = DELTA_PRINTABLE_RADIUS;
+  // The calibration radius is set to a calculated value
+  float dcr = probe_at_offset ? DELTA_PRINTABLE_RADIUS : DELTA_PRINTABLE_RADIUS - PROBING_MARGIN;
   #if HAS_PROBE_XY_OFFSET
-    // For offset probes the calibration radius is set to a safe but non-optimal value
-    dcr -= HYPOT(probe.offset_xy.x, probe.offset_xy.y);
-    if (probe_at_offset) {
-      // With probe positions both probe and nozzle need to be within the printable area
-      max_dcr = dcr;
-    }
-    // else with nozzle positions there is a risk of the probe being outside the bed
-    // but as long the nozzle stays within the printable area there is no risk of
-    // the effector crashing into the towers.
+    const float total_offset = HYPOT(probe.offset_xy.x, probe.offset_xy.y);
+    dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset;
   #endif
-
-  if (parser.seenval('R')) dcr = parser.value_float();
-  if (!WITHIN(dcr, 0, max_dcr)) {
-    SERIAL_ECHOLNPGM("?calibration (R)adius implausible.");
-    return;
-  }
+  NOMORE(dcr, DELTA_PRINTABLE_RADIUS);
+  if (parser.seenval('R')) dcr -= _MAX(parser.value_float(),0);
 
   const float calibration_precision = parser.floatval('C', 0.0f);
   if (calibration_precision < 0) {
   SERIAL_ECHOLNPGM("G33 Auto Calibrate");
 
   // Report settings
-  FSTR_P const checkingac = F("Checking... AC");
-  SERIAL_ECHOF(checkingac);
+  PGM_P const checkingac = PSTR("Checking... AC");
+  SERIAL_ECHOPGM_P(checkingac);
+  SERIAL_ECHOPGM(" at radius:", dcr);
   if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
   SERIAL_EOL();
   ui.set_status(checkingac);
 
     // Probe the points
     zero_std_dev_old = zero_std_dev;
-    if (!probe_calibration_points(z_at_pt, probe_points, towers_set, stow_after_each, probe_at_offset)) {
+    if (!probe_calibration_points(z_at_pt, probe_points, dcr, towers_set, stow_after_each, probe_at_offset)) {
       SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
       return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
     }
 
       // calculate factors
       if (_7p_9_center) dcr *= 0.9f;
-      h_factor = auto_tune_h();
-      r_factor = auto_tune_r();
-      a_factor = auto_tune_a();
-      dcr /= 0.9f;
+      h_factor = auto_tune_h(dcr);
+      r_factor = auto_tune_r(dcr);
+      a_factor = auto_tune_a(dcr);
+      if (_7p_9_center) dcr /= 0.9f;
 
       switch (probe_points) {
         case 0:

Marlin/src/inc/SanityCheck.h

   #if ENABLED(NOZZLE_AS_PROBE)
     static_assert(sanity_nozzle_to_probe_offset.x == 0 && sanity_nozzle_to_probe_offset.y == 0,
                   "NOZZLE_AS_PROBE requires the XY offsets in NOZZLE_TO_PROBE_OFFSET to both be 0.");
-  #else
+  #elif !IS_KINEMATIC
     static_assert(PROBING_MARGIN       >= 0, "PROBING_MARGIN must be >= 0.");
     static_assert(PROBING_MARGIN_BACK  >= 0, "PROBING_MARGIN_BACK must be >= 0.");
     static_assert(PROBING_MARGIN_FRONT >= 0, "PROBING_MARGIN_FRONT must be >= 0.");

Marlin/src/module/probe.cpp

 
   // On delta keep Z below clip height or do_blocking_move_to will abort
   xyz_pos_t npos = { rx, ry, TERN(DELTA, _MIN(delta_clip_start_height, current_position.z), current_position.z) };
-  if (probe_relative) {                                     // The given position is in terms of the probe
-    if (!can_reach(npos)) {
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
-      return NAN;
-    }
-    npos -= offset_xy;                                      // Get the nozzle position
+  if (!can_reach(npos, probe_relative)) {
+    if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
+    return NAN;
   }
-  else if (!position_is_reachable(npos)) return NAN;        // The given position is in terms of the nozzle
+  if (probe_relative) npos -= offset_xy;  // Get the nozzle position
 
   // Move the probe to the starting XYZ
   do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));

Marlin/src/module/probe.h

       #if HAS_PROBE_XY_OFFSET
         // Return true if the both nozzle and the probe can reach the given point.
         // Note: This won't work on SCARA since the probe offset rotates with the arm.
-        static bool can_reach(const_float_t rx, const_float_t ry) {
-          return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y) // The nozzle can go where it needs to go?
-              && position_is_reachable(rx, ry, ABS(PROBING_MARGIN));       // Can the nozzle also go near there?
+        static bool can_reach(const_float_t rx, const_float_t ry, const bool probe_relative=true) {
+          if (probe_relative) {
+            return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y) // The nozzle can go where it needs to go?
+                && position_is_reachable(rx, ry, PROBING_MARGIN);            // Can the probe also go near there?
+          }
+          else {
+            return position_is_reachable(rx, ry)
+                && position_is_reachable(rx + offset_xy.x, ry + offset_xy.y, PROBING_MARGIN);
+          }
         }
       #else
-        static bool can_reach(const_float_t rx, const_float_t ry) {
-          return position_is_reachable(rx, ry, PROBING_MARGIN);
+        static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) {
+          return position_is_reachable(rx, ry)
+              && position_is_reachable(rx, ry, PROBING_MARGIN);
         }
       #endif
 
        * Example: For a probe offset of -10,+10, then for the probe to reach 0,0 the
        *          nozzle must be be able to reach +10,-10.
        */
-      static bool can_reach(const_float_t rx, const_float_t ry) {
-        return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y)
-            && COORDINATE_OKAY(rx, min_x() - fslop, max_x() + fslop)
-            && COORDINATE_OKAY(ry, min_y() - fslop, max_y() + fslop);
+      static bool can_reach(const_float_t rx, const_float_t ry, const bool probe_relative=true) {
+        if (probe_relative) {
+          return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y)
+              && COORDINATE_OKAY(rx, min_x() - fslop, max_x() + fslop)
+              && COORDINATE_OKAY(ry, min_y() - fslop, max_y() + fslop);
+        }
+        else {
+          return position_is_reachable(rx, ry)
+              && COORDINATE_OKAY(rx + offset_xy.x, min_x() - fslop, max_x() + fslop)
+              && COORDINATE_OKAY(ry + offset_xy.y, min_y() - fslop, max_y() + fslop);
+        }
       }
 
     #endif
 
     static bool set_deployed(const bool) { return false; }
 
-    static bool can_reach(const_float_t rx, const_float_t ry) { return position_is_reachable(rx, ry); }
+    static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) { return position_is_reachable(rx, ry); }
 
   #endif
 
     #endif
   }
 
-  static bool can_reach(const xy_pos_t &pos) { return can_reach(pos.x, pos.y); }
+  static bool can_reach(const xy_pos_t &pos, const bool probe_relative=true) { return can_reach(pos.x, pos.y, probe_relative); }
 
   static bool good_bounds(const xy_pos_t &lf, const xy_pos_t &rb) {
     return (
         TERN_(DELTA, DELTA_PRINTABLE_RADIUS)
         TERN_(IS_SCARA, SCARA_PRINTABLE_RADIUS)
       );
-      static constexpr float probe_radius(const xy_pos_t &probe_offset_xy = offset_xy) {
+      static constexpr float probe_radius(const xy_pos_t &probe_offset_xy=offset_xy) {
         return printable_radius - _MAX(PROBING_MARGIN, HYPOT(probe_offset_xy.x, probe_offset_xy.y));
       }
     #endif
 
-    static constexpr float _min_x(const xy_pos_t &probe_offset_xy = offset_xy) {
+    static constexpr float _min_x(const xy_pos_t &probe_offset_xy=offset_xy) {
       return TERN(IS_KINEMATIC,
         (X_CENTER) - probe_radius(probe_offset_xy),
         _MAX((X_MIN_BED) + (PROBING_MARGIN_LEFT), (X_MIN_POS) + probe_offset_xy.x)
       );
     }
-    static constexpr float _max_x(const xy_pos_t &probe_offset_xy = offset_xy) {
+    static constexpr float _max_x(const xy_pos_t &probe_offset_xy=offset_xy) {
       return TERN(IS_KINEMATIC,
         (X_CENTER) + probe_radius(probe_offset_xy),
         _MIN((X_MAX_BED) - (PROBING_MARGIN_RIGHT), (X_MAX_POS) + probe_offset_xy.x)
       );
     }
-    static constexpr float _min_y(const xy_pos_t &probe_offset_xy = offset_xy) {
+    static constexpr float _min_y(const xy_pos_t &probe_offset_xy=offset_xy) {
       return TERN(IS_KINEMATIC,
         (Y_CENTER) - probe_radius(probe_offset_xy),
         _MAX((Y_MIN_BED) + (PROBING_MARGIN_FRONT), (Y_MIN_POS) + probe_offset_xy.y)
       );
     }
-    static constexpr float _max_y(const xy_pos_t &probe_offset_xy = offset_xy) {
+    static constexpr float _max_y(const xy_pos_t &probe_offset_xy=offset_xy) {
       return TERN(IS_KINEMATIC,
         (Y_CENTER) + probe_radius(probe_offset_xy),
         _MIN((Y_MAX_BED) - (PROBING_MARGIN_BACK), (Y_MAX_POS) + probe_offset_xy.y)