|
|
|
|
@ -584,10 +584,12 @@ static bool do_probe_move(const float z, const float fr_mm_s) {
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* @details Used by probe_pt to do a single Z probe at the current position.
|
|
|
|
|
* @brief Probe at the current XY (possibly more than once) to find the bed Z.
|
|
|
|
|
*
|
|
|
|
|
* @details Used by probe_pt to get the bed Z height at the current XY.
|
|
|
|
|
* Leaves current_position[Z_AXIS] at the height where the probe triggered.
|
|
|
|
|
*
|
|
|
|
|
* @return The raw Z position where the probe was triggered
|
|
|
|
|
* @return The Z position of the bed at the current XY or NAN on error.
|
|
|
|
|
*/
|
|
|
|
|
static float run_z_probe() {
|
|
|
|
|
|
|
|
|
|
@ -598,7 +600,7 @@ static float run_z_probe() {
|
|
|
|
|
const float z_probe_low_point = TEST(axis_known_position, Z_AXIS) ? -zprobe_zoffset + Z_PROBE_LOW_POINT : -10.0;
|
|
|
|
|
|
|
|
|
|
// Double-probing does a fast probe followed by a slow probe
|
|
|
|
|
#if MULTIPLE_PROBING == 2
|
|
|
|
|
#if TOTAL_PROBING == 2
|
|
|
|
|
|
|
|
|
|
// Do a first probe at the fast speed
|
|
|
|
|
if (do_probe_move(z_probe_low_point, MMM_TO_MMS(Z_PROBE_SPEED_FAST))) {
|
|
|
|
|
@ -609,11 +611,11 @@ static float run_z_probe() {
|
|
|
|
|
return NAN;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float first_probe_z = current_position[Z_AXIS];
|
|
|
|
|
const float first_probe_z = current_position[Z_AXIS];
|
|
|
|
|
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("1st Probe Z:", first_probe_z);
|
|
|
|
|
|
|
|
|
|
// move up to make clearance for the probe
|
|
|
|
|
// Raise to give the probe clearance
|
|
|
|
|
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_MULTI_PROBE, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
|
|
|
|
|
|
|
|
|
|
#elif Z_PROBE_SPEED_FAST != Z_PROBE_SPEED_SLOW
|
|
|
|
|
@ -622,18 +624,28 @@ static float run_z_probe() {
|
|
|
|
|
// move down quickly before doing the slow probe
|
|
|
|
|
const float z = Z_CLEARANCE_DEPLOY_PROBE + 5.0 + (zprobe_zoffset < 0 ? -zprobe_zoffset : 0);
|
|
|
|
|
if (current_position[Z_AXIS] > z) {
|
|
|
|
|
// If we don't make it to the z position (i.e. the probe triggered), move up to make clearance for the probe
|
|
|
|
|
// Probe down fast. If the probe never triggered, raise for probe clearance
|
|
|
|
|
if (!do_probe_move(z, MMM_TO_MMS(Z_PROBE_SPEED_FAST)))
|
|
|
|
|
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if MULTIPLE_PROBING > 2
|
|
|
|
|
float probes_total = 0;
|
|
|
|
|
for (uint8_t p = MULTIPLE_PROBING + 1; --p;) {
|
|
|
|
|
#ifdef EXTRA_PROBING
|
|
|
|
|
float probes[TOTAL_PROBING];
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// move down slowly to find bed
|
|
|
|
|
#if TOTAL_PROBING > 2
|
|
|
|
|
float probes_total = 0;
|
|
|
|
|
for (
|
|
|
|
|
#if EXTRA_PROBING
|
|
|
|
|
uint8_t p = 0; p < TOTAL_PROBING; p++
|
|
|
|
|
#else
|
|
|
|
|
uint8_t p = TOTAL_PROBING; p--;
|
|
|
|
|
#endif
|
|
|
|
|
)
|
|
|
|
|
#endif
|
|
|
|
|
{
|
|
|
|
|
// Probe downward slowly to find the bed
|
|
|
|
|
if (do_probe_move(z_probe_low_point, MMM_TO_MMS(Z_PROBE_SPEED_SLOW))) {
|
|
|
|
|
if (DEBUGGING(LEVELING)) {
|
|
|
|
|
DEBUG_ECHOLNPGM("SLOW Probe fail!");
|
|
|
|
|
@ -646,18 +658,54 @@ static float run_z_probe() {
|
|
|
|
|
backlash.measure_with_probe();
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if MULTIPLE_PROBING > 2
|
|
|
|
|
probes_total += current_position[Z_AXIS];
|
|
|
|
|
if (p > 1) do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_MULTI_PROBE, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
|
|
|
|
|
const float z = current_position[Z_AXIS];
|
|
|
|
|
|
|
|
|
|
#if EXTRA_PROBING
|
|
|
|
|
// Insert Z measurement into probes[]. Keep it sorted ascending.
|
|
|
|
|
for (uint8_t i = 0; i <= p; i++) { // Iterate the saved Zs to insert the new Z
|
|
|
|
|
if (i == p || probes[i] > z) { // Last index or new Z is smaller than this Z
|
|
|
|
|
for (int8_t m = p; --m >= i;) probes[m + 1] = probes[m]; // Shift items down after the insertion point
|
|
|
|
|
probes[i] = z; // Insert the new Z measurement
|
|
|
|
|
break; // Only one to insert. Done!
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
#elif TOTAL_PROBING > 2
|
|
|
|
|
probes_total += z;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if TOTAL_PROBING > 2
|
|
|
|
|
// Small Z raise after all but the last probe
|
|
|
|
|
if (p
|
|
|
|
|
#if EXTRA_PROBING
|
|
|
|
|
< TOTAL_PROBING - 1
|
|
|
|
|
#endif
|
|
|
|
|
) do_blocking_move_to_z(z + Z_CLEARANCE_MULTI_PROBE, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
|
|
|
|
|
#endif
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
#if MULTIPLE_PROBING > 2
|
|
|
|
|
#if TOTAL_PROBING > 2
|
|
|
|
|
|
|
|
|
|
#if EXTRA_PROBING
|
|
|
|
|
// Take the center value (or average the two middle values) as the median
|
|
|
|
|
static constexpr int PHALF = (TOTAL_PROBING - 1) / 2;
|
|
|
|
|
const float middle = probes[PHALF],
|
|
|
|
|
median = ((TOTAL_PROBING) & 1) ? middle : (middle + probes[PHALF + 1]) * 0.5f;
|
|
|
|
|
|
|
|
|
|
// Remove values farthest from the median
|
|
|
|
|
uint8_t min_avg_idx = 0, max_avg_idx = TOTAL_PROBING - 1;
|
|
|
|
|
for (uint8_t i = EXTRA_PROBING; i--;)
|
|
|
|
|
if (ABS(probes[max_avg_idx] - median) > ABS(probes[min_avg_idx] - median))
|
|
|
|
|
max_avg_idx--; else min_avg_idx++;
|
|
|
|
|
|
|
|
|
|
// Return the average value of all remaining probes.
|
|
|
|
|
for (uint8_t i = min_avg_idx; i <= max_avg_idx; i++)
|
|
|
|
|
probes_total += probes[i];
|
|
|
|
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
// Return the average value of all probes
|
|
|
|
|
const float measured_z = probes_total * (1.0f / (MULTIPLE_PROBING));
|
|
|
|
|
|
|
|
|
|
#elif MULTIPLE_PROBING == 2
|
|
|
|
|
#elif TOTAL_PROBING == 2
|
|
|
|
|
|
|
|
|
|
const float z2 = current_position[Z_AXIS];
|
|
|
|
|
|
|
|
|
|
|
MrMabulous
6 years ago
committed by