From 65f365333ff15b93ddbe16db18b119c717a6f7bc Mon Sep 17 00:00:00 2001 From: LVD-AC Date: Sat, 25 Nov 2017 10:59:46 +0100 Subject: [PATCH] [2.0.x] G33 probe error handling --- Marlin/src/core/macros.h | 2 +- Marlin/src/gcode/calibrate/G33.cpp | 77 ++++++++++++++--------------- Marlin/src/gcode/calibrate/M665.cpp | 6 +-- Marlin/src/module/probe.cpp | 20 ++++---- Marlin/src/module/probe.h | 2 +- 5 files changed, 51 insertions(+), 56 deletions(-) diff --git a/Marlin/src/core/macros.h b/Marlin/src/core/macros.h index a925362b1..aed085021 100644 --- a/Marlin/src/core/macros.h +++ b/Marlin/src/core/macros.h @@ -96,7 +96,7 @@ // Macros for bit masks #ifndef _BV - #define _BV(B) (1UL<<(B)) + #define _BV(n) (1<<(n)) #endif #define TEST(n,b) (((n)&_BV(b))!=0) #define SBI(n,b) (n |= _BV(b)) diff --git a/Marlin/src/gcode/calibrate/G33.cpp b/Marlin/src/gcode/calibrate/G33.cpp index 06240eb30..ec260ff4a 100644 --- a/Marlin/src/gcode/calibrate/G33.cpp +++ b/Marlin/src/gcode/calibrate/G33.cpp @@ -135,6 +135,15 @@ static void G33_cleanup( #endif } +inline float calibration_probe(const float nx, const float ny, const bool stow) { + #if HAS_BED_PROBE + return probe_pt(nx, ny, stow, 0, false); + #else + UNUSED(stow); + return lcd_probe_pt(nx, ny); + #endif +} + static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, const bool towers_set, const bool stow_after_each) { const bool _0p_calibration = probe_points == 0, _1p_calibration = probe_points == 1, @@ -153,23 +162,13 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, _7p_6_centre = probe_points >= 5 && probe_points <= 7, _7p_9_centre = probe_points >= 8; - #if HAS_BED_PROBE - const float dx = (X_PROBE_OFFSET_FROM_EXTRUDER), - dy = (Y_PROBE_OFFSET_FROM_EXTRUDER); - #endif - LOOP_CAL_ALL(axis) z_at_pt[axis] = 0.0; if (!_0p_calibration) { if (!_7p_no_intermediates && !_7p_4_intermediates && !_7p_11_intermediates) { // probe the center - z_at_pt[CEN] += - #if HAS_BED_PROBE - probe_pt(dx, dy, stow_after_each, 1, false) - #else - lcd_probe_pt(0, 0) - #endif - ; + z_at_pt[CEN] += calibration_probe(0, 0, stow_after_each); + if (isnan(z_at_pt[CEN])) return NAN; } if (_7p_calibration) { // probe extra center points @@ -178,14 +177,9 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, I_LOOP_CAL_PT(axis, start, steps) { const float a = RADIANS(210 + (360 / NPP) * (axis - 1)), r = delta_calibration_radius * 0.1; - z_at_pt[CEN] += - #if HAS_BED_PROBE - probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1, false) - #else - lcd_probe_pt(cos(a) * r, sin(a) * r) - #endif - ; - } + z_at_pt[CEN] += calibration_probe(cos(a) * r, sin(a) * r, stow_after_each); + if (isnan(z_at_pt[CEN])) return NAN; + } z_at_pt[CEN] /= float(_7p_2_intermediates ? 7 : probe_points); } @@ -206,14 +200,9 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, for (int8_t circle = -offset; circle <= offset; circle++) { const float a = RADIANS(210 + (360 / NPP) * (axis - 1)), r = delta_calibration_radius * (1 + 0.1 * (zig_zag ? circle : - circle)), - interpol = FMOD(axis, 1); - const float z_temp = - #if HAS_BED_PROBE - probe_pt(cos(a) * r + dx, sin(a) * r + dy, stow_after_each, 1, false) - #else - lcd_probe_pt(cos(a) * r, sin(a) * r) - #endif - ; + interpol = fmod(axis, 1); + const float z_temp = calibration_probe(cos(a) * r, sin(a) * r, stow_after_each); + if (isnan(z_temp)) return NAN; // split probe point to neighbouring calibration points z_at_pt[uint8_t(round(axis - interpol + NPP - 1)) % NPP + 1] += z_temp * sq(cos(RADIANS(interpol * 90))); z_at_pt[uint8_t(round(axis - interpol)) % NPP + 1] += z_temp * sq(sin(RADIANS(interpol * 90))); @@ -243,7 +232,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, #if HAS_BED_PROBE - static void G33_auto_tune() { + static bool G33_auto_tune() { float z_at_pt[NPP + 1] = { 0.0 }, z_at_pt_base[NPP + 1] = { 0.0 }, z_temp, h_fac = 0.0, r_fac = 0.0, a_fac = 0.0, norm = 0.8; @@ -257,7 +246,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, SERIAL_PROTOCOLPGM("AUTO TUNE baseline"); SERIAL_EOL(); - probe_G33_points(z_at_pt_base, 3, true, false); + if (isnan(probe_G33_points(z_at_pt_base, 3, true, false))) return false; print_G33_results(z_at_pt_base, true, true); LOOP_XYZ(axis) { @@ -272,7 +261,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, SERIAL_CHAR(tolower(axis_codes[axis])); SERIAL_EOL(); - probe_G33_points(z_at_pt, 3, true, false); + if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false; LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis]; print_G33_results(z_at_pt, true, true); delta_endstop_adj[axis] += 1.0; @@ -303,7 +292,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, SERIAL_PROTOCOLPGM("Tuning R"); SERIAL_PROTOCOL(zig_zag == -1 ? "-" : "+"); SERIAL_EOL(); - probe_G33_points(z_at_pt, 3, true, false); + if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false; LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis]; print_G33_results(z_at_pt, true, true); delta_radius -= 1.0 * zig_zag; @@ -330,7 +319,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, SERIAL_CHAR(tolower(axis_codes[axis])); SERIAL_EOL(); - probe_G33_points(z_at_pt, 3, true, false); + if (isnan(probe_G33_points(z_at_pt, 3, true, false))) return false; LOOP_CAL_ALL(axis) z_at_pt[axis] -= z_at_pt_base[axis]; print_G33_results(z_at_pt, true, true); @@ -365,6 +354,7 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, SERIAL_EOL(); SERIAL_PROTOCOLPGM("Copy these values to Configuration.h"); SERIAL_EOL(); + return true; } #endif // HAS_BED_PROBE @@ -392,8 +382,9 @@ static float probe_G33_points(float z_at_pt[NPP + 1], const int8_t probe_points, * * Vn Verbose level: * V0 Dry-run mode. Report settings and probe results. No calibration. - * V1 Report settings - * V2 Report settings and probe results + * V1 Report start and end settings only + * V2 Report settings at each iteration + * V3 Report settings and probe results * * E Engage the probe for each point */ @@ -406,12 +397,12 @@ void GcodeSuite::G33() { } const int8_t verbose_level = parser.byteval('V', 1); - if (!WITHIN(verbose_level, 0, 2)) { - SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-2)."); + if (!WITHIN(verbose_level, 0, 3)) { + SERIAL_PROTOCOLLNPGM("?(V)erbose level is implausible (0-3)."); return; } - const float calibration_precision = parser.floatval('C'); + const float calibration_precision = parser.floatval('C', 0.0); if (calibration_precision < 0) { SERIAL_PROTOCOLLNPGM("?(C)alibration precision is implausible (>=0)."); return; @@ -519,6 +510,11 @@ void GcodeSuite::G33() { // Probe the points zero_std_dev = probe_G33_points(z_at_pt, probe_points, towers_set, stow_after_each); + if (isnan(zero_std_dev)) { + SERIAL_PROTOCOLPGM("Correct delta_radius with M665 R or end-stops with M666 X Y Z"); + SERIAL_EOL(); + return G33_CLEANUP(); + } // Solve matrices @@ -632,7 +628,7 @@ void GcodeSuite::G33() { // print report - if (verbose_level != 1) + if (verbose_level > 2) print_G33_results(z_at_pt, _tower_results, _opposite_results); if (verbose_level != 0) { // !dry run @@ -672,7 +668,8 @@ void GcodeSuite::G33() { SERIAL_PROTOCOL_F(zero_std_dev, 3); SERIAL_EOL(); lcd_setstatus(mess); - print_G33_settings(_endstop_results, _angle_results); + if (verbose_level > 1) + print_G33_settings(_endstop_results, _angle_results); } } else { // dry run diff --git a/Marlin/src/gcode/calibrate/M665.cpp b/Marlin/src/gcode/calibrate/M665.cpp index 2f7b4bd2d..902556460 100644 --- a/Marlin/src/gcode/calibrate/M665.cpp +++ b/Marlin/src/gcode/calibrate/M665.cpp @@ -30,7 +30,6 @@ #if ENABLED(DELTA) #include "../../module/delta.h" - /** * M665: Set delta configurations * @@ -44,10 +43,7 @@ * Z = Rotate A and B by this angle */ void GcodeSuite::M665() { - if (parser.seen('H')) { - delta_height = parser.value_linear_units(); - update_software_endstops(Z_AXIS); - } + if (parser.seen('H')) delta_height = parser.value_linear_units(); if (parser.seen('L')) delta_diagonal_rod = parser.value_linear_units(); if (parser.seen('R')) delta_radius = parser.value_linear_units(); if (parser.seen('S')) delta_segments_per_second = parser.value_float(); diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index a6591db58..fd53a671d 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -564,7 +564,7 @@ static float run_z_probe() { } #endif - return current_position[Z_AXIS] + zprobe_zoffset; + return current_position[Z_AXIS]; } /** @@ -576,7 +576,7 @@ static float run_z_probe() { * - Raise to the BETWEEN height * - Return the probed Z position */ -float probe_pt(const float &rx, const float &ry, const bool stow, const uint8_t verbose_level, const bool printable/*=true*/) { +float probe_pt(const float &rx, const float &ry, const bool stow, const uint8_t verbose_level, const bool probe_relative/*=true*/) { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { SERIAL_ECHOPAIR(">>> probe_pt(", LOGICAL_X_POSITION(rx)); @@ -587,12 +587,14 @@ float probe_pt(const float &rx, const float &ry, const bool stow, const uint8_t } #endif - const float nx = rx - (X_PROBE_OFFSET_FROM_EXTRUDER), ny = ry - (Y_PROBE_OFFSET_FROM_EXTRUDER); - - if (!printable - ? !position_is_reachable(nx, ny) - : !position_is_reachable_by_probe(rx, ry) - ) return NAN; + // TODO: Adapt for SCARA, where the offset rotates + float nx = rx, ny = ry; + if (probe_relative) { + if (!position_is_reachable_by_probe(rx, ry)) return NAN; // The given position is in terms of the probe + nx -= (X_PROBE_OFFSET_FROM_EXTRUDER); // Get the nozzle position + ny -= (Y_PROBE_OFFSET_FROM_EXTRUDER); + } + else if (!position_is_reachable(nx, ny)) return NAN; // The given position is in terms of the nozzle const float nz = #if ENABLED(DELTA) @@ -611,7 +613,7 @@ float probe_pt(const float &rx, const float &ry, const bool stow, const uint8_t float measured_z = NAN; if (!DEPLOY_PROBE()) { - measured_z = run_z_probe(); + measured_z = run_z_probe() + zprobe_zoffset; if (!stow) do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST)); diff --git a/Marlin/src/module/probe.h b/Marlin/src/module/probe.h index be7a3390a..d2b326d3a 100644 --- a/Marlin/src/module/probe.h +++ b/Marlin/src/module/probe.h @@ -32,7 +32,7 @@ #if HAS_BED_PROBE extern float zprobe_zoffset; bool set_probe_deployed(const bool deploy); - float probe_pt(const float &rx, const float &ry, const bool, const uint8_t, const bool printable=true); + float probe_pt(const float &rx, const float &ry, const bool, const uint8_t, const bool probe_relative=true); #define DEPLOY_PROBE() set_probe_deployed(true) #define STOW_PROBE() set_probe_deployed(false) #else