From 629498f8d43be6675674671927456b8b6b4ac47b Mon Sep 17 00:00:00 2001 From: Luc Van Daele Date: Tue, 16 Nov 2021 16:24:53 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9B=20Fix=20G33,=20Delta=20radii,=20re?= =?UTF-8?q?achable=20(#22795)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/LPC1768/fast_pwm.cpp | 2 +- Marlin/src/gcode/calibrate/G33.cpp | 71 ++++++++++++----------------- Marlin/src/inc/SanityCheck.h | 2 +- Marlin/src/module/probe.cpp | 11 ++--- Marlin/src/module/probe.h | 46 ++++++++++++------- 5 files changed, 66 insertions(+), 66 deletions(-) diff --git a/Marlin/src/HAL/LPC1768/fast_pwm.cpp b/Marlin/src/HAL/LPC1768/fast_pwm.cpp index 197abb3310..eae0e36b0b 100644 --- a/Marlin/src/HAL/LPC1768/fast_pwm.cpp +++ b/Marlin/src/HAL/LPC1768/fast_pwm.cpp @@ -26,7 +26,7 @@ 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 } diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp index 779ae99d0a..a4b9aec01b 100644 --- a/Marlin/src/gcode/calibrate/G33.cpp +++ b/Marlin/src/gcode/calibrate/G33.cpp @@ -69,8 +69,6 @@ enum CalEnum : char { // the 7 main calibration points - 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)); @@ -177,7 +175,7 @@ static float std_dev_points(float z_pt[NPP + 1], const bool _0p_cal, const bool */ 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); @@ -187,7 +185,7 @@ static float calibration_probe(const xy_pos_t &xy, const bool stow, const bool p /** * - 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, @@ -271,7 +269,7 @@ static bool probe_calibration_points(float z_pt[NPP + 1], const int8_t probe_poi * - 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) { @@ -283,7 +281,7 @@ static void reverse_kinematics_probe_points(float z_pt[NPP + 1], abc_float_t mm_ } } -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) @@ -302,19 +300,19 @@ static void forward_kinematics_probe_points(abc_float_t mm_at_pt_axis[NPP + 1], 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; @@ -324,23 +322,23 @@ static void calc_kinematics_diff_probe_points(float z_pt[NPP + 1], abc_float_t d 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 }; @@ -348,7 +346,7 @@ static float auto_tune_a() { 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; @@ -370,7 +368,7 @@ static float auto_tune_a() { * 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 * @@ -386,7 +384,7 @@ static float auto_tune_a() { * * 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.) @@ -404,27 +402,17 @@ void GcodeSuite::G33() { 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) { @@ -475,8 +463,9 @@ void GcodeSuite::G33() { 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); @@ -496,7 +485,7 @@ void GcodeSuite::G33() { // 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)); } @@ -536,10 +525,10 @@ void GcodeSuite::G33() { // 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: diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 866b86ef6b..4186e99467 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -1635,7 +1635,7 @@ static_assert(Y_MAX_LENGTH >= Y_BED_SIZE, "Movement bounds (Y_MIN_POS, Y_MAX_POS #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."); diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index 984a3aabb5..89a043d0f5 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -768,14 +768,11 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai // 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)); diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h index f9275ba9fd..3c97afcb89 100644 --- a/Marlin/src/module/probe.h +++ b/Marlin/src/module/probe.h @@ -77,13 +77,20 @@ public: #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 @@ -96,10 +103,17 @@ public: * 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 @@ -120,7 +134,7 @@ public: 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 @@ -132,7 +146,7 @@ public: #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 ( @@ -161,30 +175,30 @@ public: 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)