From ac76101ec328fe9314a229a1bb94e01bbd87a3fb Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 11 Aug 2017 16:59:32 -0500 Subject: [PATCH] Eliminate goto in gcode_M48 --- Marlin/Marlin_main.cpp | 260 +++++++++++++++++++++-------------------- 1 file changed, 132 insertions(+), 128 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 981ba84e5..a94d778be 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -7038,150 +7038,154 @@ inline void gcode_M42() { // Move to the first point, deploy, and probe const float t = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, verbose_level); - if (nan_error(t)) goto FAIL; + bool probing_good = !isnan(t); - randomSeed(millis()); + if (probing_good) { + randomSeed(millis()); - for (uint8_t n = 0; n < n_samples; n++) { - if (n_legs) { - const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise - float angle = random(0.0, 360.0); - const float radius = random( - #if ENABLED(DELTA) - 0.1250000000 * (DELTA_PROBEABLE_RADIUS), - 0.3333333333 * (DELTA_PROBEABLE_RADIUS) - #else - 5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE) - #endif - ); + for (uint8_t n = 0; n < n_samples; n++) { + if (n_legs) { + const int dir = (random(0, 10) > 5.0) ? -1 : 1; // clockwise or counter clockwise + float angle = random(0.0, 360.0); + const float radius = random( + #if ENABLED(DELTA) + 0.1250000000 * (DELTA_PROBEABLE_RADIUS), + 0.3333333333 * (DELTA_PROBEABLE_RADIUS) + #else + 5.0, 0.125 * min(X_BED_SIZE, Y_BED_SIZE) + #endif + ); - if (verbose_level > 3) { - SERIAL_ECHOPAIR("Starting radius: ", radius); - SERIAL_ECHOPAIR(" angle: ", angle); - SERIAL_ECHOPGM(" Direction: "); - if (dir > 0) SERIAL_ECHOPGM("Counter-"); - SERIAL_ECHOLNPGM("Clockwise"); - } - - for (uint8_t l = 0; l < n_legs - 1; l++) { - double delta_angle; - - if (schizoid_flag) - // The points of a 5 point star are 72 degrees apart. We need to - // skip a point and go to the next one on the star. - delta_angle = dir * 2.0 * 72.0; - - else - // If we do this line, we are just trying to move further - // around the circle. - delta_angle = dir * (float) random(25, 45); - - angle += delta_angle; - - while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the - angle -= 360.0; // Arduino documentation says the trig functions should not be given values - while (angle < 0.0) // outside of this range. It looks like they behave correctly with - angle += 360.0; // numbers outside of the range, but just to be safe we clamp them. - - X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius; - Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius; - - #if DISABLED(DELTA) - X_current = constrain(X_current, X_MIN_POS, X_MAX_POS); - Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS); - #else - // If we have gone out too far, we can do a simple fix and scale the numbers - // back in closer to the origin. - while (!position_is_reachable_by_probe_xy(X_current, Y_current)) { - X_current *= 0.8; - Y_current *= 0.8; - if (verbose_level > 3) { - SERIAL_ECHOPAIR("Pulling point towards center:", X_current); - SERIAL_ECHOLNPAIR(", ", Y_current); - } - } - #endif if (verbose_level > 3) { - SERIAL_PROTOCOLPGM("Going to:"); - SERIAL_ECHOPAIR(" X", X_current); - SERIAL_ECHOPAIR(" Y", Y_current); - SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]); + SERIAL_ECHOPAIR("Starting radius: ", radius); + SERIAL_ECHOPAIR(" angle: ", angle); + SERIAL_ECHOPGM(" Direction: "); + if (dir > 0) SERIAL_ECHOPGM("Counter-"); + SERIAL_ECHOLNPGM("Clockwise"); } - do_blocking_move_to_xy(X_current, Y_current); - } // n_legs loop - } // n_legs - // Probe a single point - sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0); - if (nan_error(sample_set[n])) goto FAIL; + for (uint8_t l = 0; l < n_legs - 1; l++) { + double delta_angle; - /** - * Get the current mean for the data points we have so far - */ - double sum = 0.0; - for (uint8_t j = 0; j <= n; j++) sum += sample_set[j]; - mean = sum / (n + 1); + if (schizoid_flag) + // The points of a 5 point star are 72 degrees apart. We need to + // skip a point and go to the next one on the star. + delta_angle = dir * 2.0 * 72.0; - NOMORE(min, sample_set[n]); - NOLESS(max, sample_set[n]); + else + // If we do this line, we are just trying to move further + // around the circle. + delta_angle = dir * (float) random(25, 45); - /** - * Now, use that mean to calculate the standard deviation for the - * data points we have so far - */ - sum = 0.0; - for (uint8_t j = 0; j <= n; j++) - sum += sq(sample_set[j] - mean); + angle += delta_angle; - sigma = SQRT(sum / (n + 1)); - if (verbose_level > 0) { - if (verbose_level > 1) { - SERIAL_PROTOCOL(n + 1); - SERIAL_PROTOCOLPGM(" of "); - SERIAL_PROTOCOL((int)n_samples); - SERIAL_PROTOCOLPGM(": z: "); - SERIAL_PROTOCOL_F(sample_set[n], 3); - if (verbose_level > 2) { - SERIAL_PROTOCOLPGM(" mean: "); - SERIAL_PROTOCOL_F(mean, 4); - SERIAL_PROTOCOLPGM(" sigma: "); - SERIAL_PROTOCOL_F(sigma, 6); - SERIAL_PROTOCOLPGM(" min: "); - SERIAL_PROTOCOL_F(min, 3); - SERIAL_PROTOCOLPGM(" max: "); - SERIAL_PROTOCOL_F(max, 3); - SERIAL_PROTOCOLPGM(" range: "); - SERIAL_PROTOCOL_F(max-min, 3); + while (angle > 360.0) // We probably do not need to keep the angle between 0 and 2*PI, but the + angle -= 360.0; // Arduino documentation says the trig functions should not be given values + while (angle < 0.0) // outside of this range. It looks like they behave correctly with + angle += 360.0; // numbers outside of the range, but just to be safe we clamp them. + + X_current = X_probe_location - (X_PROBE_OFFSET_FROM_EXTRUDER) + cos(RADIANS(angle)) * radius; + Y_current = Y_probe_location - (Y_PROBE_OFFSET_FROM_EXTRUDER) + sin(RADIANS(angle)) * radius; + + #if DISABLED(DELTA) + X_current = constrain(X_current, X_MIN_POS, X_MAX_POS); + Y_current = constrain(Y_current, Y_MIN_POS, Y_MAX_POS); + #else + // If we have gone out too far, we can do a simple fix and scale the numbers + // back in closer to the origin. + while (!position_is_reachable_by_probe_xy(X_current, Y_current)) { + X_current *= 0.8; + Y_current *= 0.8; + if (verbose_level > 3) { + SERIAL_ECHOPAIR("Pulling point towards center:", X_current); + SERIAL_ECHOLNPAIR(", ", Y_current); + } + } + #endif + if (verbose_level > 3) { + SERIAL_PROTOCOLPGM("Going to:"); + SERIAL_ECHOPAIR(" X", X_current); + SERIAL_ECHOPAIR(" Y", Y_current); + SERIAL_ECHOLNPAIR(" Z", current_position[Z_AXIS]); + } + do_blocking_move_to_xy(X_current, Y_current); + } // n_legs loop + } // n_legs + + // Probe a single point + sample_set[n] = probe_pt(X_probe_location, Y_probe_location, stow_probe_after_each, 0); + + // Break the loop if the probe fails + probing_good = !isnan(sample_set[n]); + if (!probing_good) break; + + /** + * Get the current mean for the data points we have so far + */ + double sum = 0.0; + for (uint8_t j = 0; j <= n; j++) sum += sample_set[j]; + mean = sum / (n + 1); + + NOMORE(min, sample_set[n]); + NOLESS(max, sample_set[n]); + + /** + * Now, use that mean to calculate the standard deviation for the + * data points we have so far + */ + sum = 0.0; + for (uint8_t j = 0; j <= n; j++) + sum += sq(sample_set[j] - mean); + + sigma = SQRT(sum / (n + 1)); + if (verbose_level > 0) { + if (verbose_level > 1) { + SERIAL_PROTOCOL(n + 1); + SERIAL_PROTOCOLPGM(" of "); + SERIAL_PROTOCOL((int)n_samples); + SERIAL_PROTOCOLPGM(": z: "); + SERIAL_PROTOCOL_F(sample_set[n], 3); + if (verbose_level > 2) { + SERIAL_PROTOCOLPGM(" mean: "); + SERIAL_PROTOCOL_F(mean, 4); + SERIAL_PROTOCOLPGM(" sigma: "); + SERIAL_PROTOCOL_F(sigma, 6); + SERIAL_PROTOCOLPGM(" min: "); + SERIAL_PROTOCOL_F(min, 3); + SERIAL_PROTOCOLPGM(" max: "); + SERIAL_PROTOCOL_F(max, 3); + SERIAL_PROTOCOLPGM(" range: "); + SERIAL_PROTOCOL_F(max-min, 3); + } + SERIAL_EOL(); } - SERIAL_EOL(); } - } - } // End of probe loop - - if (STOW_PROBE()) goto FAIL; - - SERIAL_PROTOCOLPGM("Finished!"); - SERIAL_EOL(); - - if (verbose_level > 0) { - SERIAL_PROTOCOLPGM("Mean: "); - SERIAL_PROTOCOL_F(mean, 6); - SERIAL_PROTOCOLPGM(" Min: "); - SERIAL_PROTOCOL_F(min, 3); - SERIAL_PROTOCOLPGM(" Max: "); - SERIAL_PROTOCOL_F(max, 3); - SERIAL_PROTOCOLPGM(" Range: "); - SERIAL_PROTOCOL_F(max-min, 3); - SERIAL_EOL(); + } // n_samples loop } - SERIAL_PROTOCOLPGM("Standard Deviation: "); - SERIAL_PROTOCOL_F(sigma, 6); - SERIAL_EOL(); - SERIAL_EOL(); + STOW_PROBE(); - FAIL: + if (probing_good) { + SERIAL_PROTOCOLLNPGM("Finished!"); + + if (verbose_level > 0) { + SERIAL_PROTOCOLPGM("Mean: "); + SERIAL_PROTOCOL_F(mean, 6); + SERIAL_PROTOCOLPGM(" Min: "); + SERIAL_PROTOCOL_F(min, 3); + SERIAL_PROTOCOLPGM(" Max: "); + SERIAL_PROTOCOL_F(max, 3); + SERIAL_PROTOCOLPGM(" Range: "); + SERIAL_PROTOCOL_F(max-min, 3); + SERIAL_EOL(); + } + + SERIAL_PROTOCOLPGM("Standard Deviation: "); + SERIAL_PROTOCOL_F(sigma, 6); + SERIAL_EOL(); + SERIAL_EOL(); + } clean_up_after_endstop_or_probe_move();