inline bool SCARA_move_to_cal(const uint8_t delta_a, const uint8_t delta_b) {
if (IsRunning()) {
forward_kinematics_SCARA(delta_a, delta_b);
do_blocking_move_to_xy(cartes[X_AXIS], cartes[Y_AXIS]);
return true;
}
return false;
}
inline void home_z_safely() {
// Disallow Z homing if X or Y are unknown
if (!TEST(axis_known_position, X_AXIS) || !TEST(axis_known_position, Y_AXIS)) {
LCD_MESSAGEPGM(MSG_ERR_Z_HOMING);
SERIAL_ECHO_MSG(MSG_ERR_Z_HOMING);
return;
}
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("Z_SAFE_HOMING >>>");
#endif
sync_plan_position();
/**
* Move the Z probe (or just the nozzle) to the safe homing point
*/
destination[X_AXIS] = Z_SAFE_HOMING_X_POINT;
destination[Y_AXIS] = Z_SAFE_HOMING_Y_POINT;
destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height
#if HOMING_Z_WITH_PROBE
destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER;
destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER;
#endif
if (position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination);
#endif
// This causes the carriage on Dual X to unpark
#if ENABLED(DUAL_X_CARRIAGE)
active_extruder_parked = false;
#endif
#if ENABLED(SENSORLESS_HOMING)
safe_delay(500); // Short delay needed to settle
#endif
do_blocking_move_to_xy(destination[X_AXIS], destination[Y_AXIS]);
homeaxis(Z_AXIS);
}
else {
LCD_MESSAGEPGM(MSG_ZPROBE_OUT);
SERIAL_ECHO_MSG(MSG_ZPROBE_OUT);
}
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< Z_SAFE_HOMING");
#endif
}
static void quick_home_xy() {
// Pretend the current position is 0,0
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
sync_plan_position();
const int x_axis_home_dir =
#if ENABLED(DUAL_X_CARRIAGE)
x_home_dir(active_extruder)
#else
home_dir(X_AXIS)
#endif
;
const float mlx = max_length(X_AXIS),
mly = max_length(Y_AXIS),
mlratio = mlx > mly ? mly / mlx : mlx / mly,
fr_mm_s = MIN(homing_feedrate(X_AXIS), homing_feedrate(Y_AXIS)) * SQRT(sq(mlratio) + 1.0);
#if ENABLED(SENSORLESS_HOMING)
sensorless_t stealth_states { false, false, false, false, false, false, false };
stealth_states.x = tmc_enable_stallguard(stepperX);
stealth_states.y = tmc_enable_stallguard(stepperY);
#if AXIS_HAS_STALLGUARD(X2)
stealth_states.x2 = tmc_enable_stallguard(stepperX2);
#endif
#if AXIS_HAS_STALLGUARD(Y2)
stealth_states.y2 = tmc_enable_stallguard(stepperY2);
#endif
#endif
do_blocking_move_to_xy(1.5 * mlx * x_axis_home_dir, 1.5 * mly * home_dir(Y_AXIS), fr_mm_s);
endstops.validate_homing_move();
current_position[X_AXIS] = current_position[Y_AXIS] = 0.0;
#if ENABLED(SENSORLESS_HOMING)
tmc_disable_stallguard(stepperX, stealth_states.x);
tmc_disable_stallguard(stepperY, stealth_states.y);
#if AXIS_HAS_STALLGUARD(X2)
tmc_disable_stallguard(stepperX2, stealth_states.x2);
#endif
#if AXIS_HAS_STALLGUARD(Y2)
tmc_disable_stallguard(stepperY2, stealth_states.y2);
#endif
#endif
}
/**
* - Move to the given XY
* - Deploy the probe, if not already deployed
* - Probe the bed, get the Z position
* - Depending on the 'stow' flag
* - Stow the probe, or
* - Raise to the BETWEEN height
* - Return the probed Z position
*/
float probe_pt(const float &lx, const float &ly, const bool stow, const uint8_t verbose_level, const bool printable/*=true*/) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR(">>> probe_pt(", lx);
SERIAL_ECHOPAIR(", ", ly);
SERIAL_ECHOPAIR(", ", stow ? "" : "no ");
SERIAL_ECHOLNPGM("stow)");
DEBUG_POS("", current_position);
}
#endif
const float nx = lx - (X_PROBE_OFFSET_FROM_EXTRUDER), ny = ly - (Y_PROBE_OFFSET_FROM_EXTRUDER);
if (printable
? !position_is_reachable_xy(nx, ny)
: !position_is_reachable_by_probe_xy(lx, ly)
) return NAN;
const float old_feedrate_mm_s = feedrate_mm_s;
#if ENABLED(DELTA)
if (current_position[Z_AXIS] > delta_clip_start_height)
do_blocking_move_to_z(delta_clip_start_height);
#endif
#if HAS_SOFTWARE_ENDSTOPS
// Store the status of the soft endstops and disable if we're probing a non-printable location
static bool enable_soft_endstops = soft_endstops_enabled;
if (!printable) soft_endstops_enabled = false;
#endif
feedrate_mm_s = XY_PROBE_FEEDRATE_MM_S;
// Move the probe to the given XY
do_blocking_move_to_xy(nx, ny);
float measured_z = NAN;
if (!DEPLOY_PROBE()) {
measured_z = run_z_probe(printable);
if (!stow)
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
else
if (STOW_PROBE()) measured_z = NAN;
}
#if HAS_SOFTWARE_ENDSTOPS
// Restore the soft endstop status
soft_endstops_enabled = enable_soft_endstops;
#endif
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Bed X: ");
SERIAL_PROTOCOL_F(lx, 3);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL_F(ly, 3);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL_F(measured_z, 3);
SERIAL_EOL();
}
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< probe_pt");
#endif
feedrate_mm_s = old_feedrate_mm_s;
if (isnan(measured_z)) {
LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
}
return measured_z;
}
/**
* M48: Z probe repeatability measurement function.
*
* Usage:
* M48 <P#> <X#> <Y#> <V#> <E> <L#> <S>
* P = Number of sampled points (4-50, default 10)
* X = Sample X position
* Y = Sample Y position
* V = Verbose level (0-4, default=1)
* E = Engage Z probe for each reading
* L = Number of legs of movement before probe
* S = Schizoid (Or Star if you prefer)
*
* This function requires the machine to be homed before invocation.
*/
void GcodeSuite::M48() {
if (axis_unhomed_error()) return;
const int8_t verbose_level = parser.byteval('V', 1);
if (!WITHIN(verbose_level, 0, 4)) {
SERIAL_ECHOLNPGM("?(V)erbose level is implausible (0-4).");
return;
}
if (verbose_level > 0)
SERIAL_ECHOLNPGM("M48 Z-Probe Repeatability Test");
const int8_t n_samples = parser.byteval('P', 10);
if (!WITHIN(n_samples, 4, 50)) {
SERIAL_ECHOLNPGM("?Sample size not plausible (4-50).");
return;
}
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
float X_current = current_position[X_AXIS],
Y_current = current_position[Y_AXIS];
const float X_probe_location = parser.linearval('X', X_current + X_PROBE_OFFSET_FROM_EXTRUDER),
Y_probe_location = parser.linearval('Y', Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER);
if (!position_is_reachable_by_probe(X_probe_location, Y_probe_location)) {
SERIAL_ECHOLNPGM("? (X,Y) out of bounds.");
return;
}
bool seen_L = parser.seen('L');
uint8_t n_legs = seen_L ? parser.value_byte() : 0;
if (n_legs > 15) {
SERIAL_ECHOLNPGM("?Number of legs in movement not plausible (0-15).");
return;
}
if (n_legs == 1) n_legs = 2;
const bool schizoid_flag = parser.boolval('S');
if (schizoid_flag && !seen_L) n_legs = 7;
/**
* Now get everything to the specified probe point So we can safely do a
* probe to get us close to the bed. If the Z-Axis is far from the bed,
* we don't want to use that as a starting point for each probe.
*/
if (verbose_level > 2)
SERIAL_ECHOLNPGM("Positioning the probe...");
// Disable bed level correction in M48 because we want the raw data when we probe
#if HAS_LEVELING
const bool was_enabled = planner.leveling_active;
set_bed_leveling_enabled(false);
#endif
setup_for_endstop_or_probe_move();
float mean = 0.0, sigma = 0.0, min = 99999.9, max = -99999.9, sample_set[n_samples];
// Move to the first point, deploy, and probe
const float t = probe_pt(X_probe_location, Y_probe_location, raise_after, verbose_level);
bool probing_good = !isnan(t);
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, 360);
const float radius = random(
#if ENABLED(DELTA)
(int) (0.1250000000 * (DELTA_PRINTABLE_RADIUS)),
(int) (0.3333333333 * (DELTA_PRINTABLE_RADIUS))
#else
(int) 5.0, (int) (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++) {
float 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(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_ECHOPGM("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, raise_after, 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
*/
float 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_ECHO(n + 1);
SERIAL_ECHOPAIR(" of ", (int)n_samples);
SERIAL_ECHOPAIR_F(": z: ", sample_set[n], 3);
if (verbose_level > 2) {
SERIAL_ECHOPAIR_F(" mean: ", mean, 4);
SERIAL_ECHOPAIR_F(" sigma: ", sigma, 6);
SERIAL_ECHOPAIR_F(" min: ", min, 3);
SERIAL_ECHOPAIR_F(" max: ", max, 3);
SERIAL_ECHOPAIR_F(" range: ", max-min, 3);
}
SERIAL_EOL();
}
}
} // n_samples loop
}
STOW_PROBE();
if (probing_good) {
SERIAL_ECHOLNPGM("Finished!");
if (verbose_level > 0) {
SERIAL_ECHOPAIR_F("Mean: ", mean, 6);
SERIAL_ECHOPAIR_F(" Min: ", min, 3);
SERIAL_ECHOPAIR_F(" Max: ", max, 3);
SERIAL_ECHOLNPAIR_F(" Range: ", max-min, 3);
}
SERIAL_ECHOLNPAIR_F("Standard Deviation: ", sigma, 6);
SERIAL_EOL();
}
clean_up_after_endstop_or_probe_move();
// Re-enable bed level correction if it had been on
#if HAS_LEVELING
set_bed_leveling_enabled(was_enabled);
#endif
report_current_position();
}
