bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

🐛 Fix G33, Delta radii, reachable (#22795)

Browse Source
This commit is contained in:
Luc Van Daele 2021-11-16 16:24:53 +01:00 committed by Scott Lahteine
parent 39a81d167e
commit 656034d2d9
5 changed files with 66 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
Marlin/src/gcode/calibrate/G33.cpp
View File

@ -69,8 +69,6 @@ enum CalEnum : char { // the 7 main calibration points -
float lcd_probe_pt(const xy_pos_t &xy); float lcd_probe_pt(const xy_pos_t &xy);
float dcr;
void ac_home() { void ac_home() {
endstops.enable(true); endstops.enable(true);
TERN_(HAS_DELTA_SENSORLESS_PROBING, probe.set_homing_current(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) { static float calibration_probe(const xy_pos_t &xy, const bool stow, const bool probe_at_offset) {
#if HAS_BED_PROBE #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 #else
UNUSED(stow); UNUSED(stow);
return lcd_probe_pt(xy); 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 * - 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, const bool _0p_calibration = probe_points == 0,
_1p_calibration = probe_points == 1 || probe_points == -1, _1p_calibration = probe_points == 1 || probe_points == -1,
_4p_calibration = probe_points == 2, _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 * - formulae for approximative forward kinematics in the end-stop displacement matrix
* - definition of the matrix scaling parameters * - 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}; xyz_pos_t pos{0};
LOOP_CAL_ALL(rad) { 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; 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) #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); 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]; const float z_center = z_pt[CEN];
abc_float_t diff_mm_at_pt_axis[NPP + 1], new_mm_at_pt_axis[NPP + 1]; 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_radius += delta_r;
delta_tower_angle_trim += delta_t; delta_tower_angle_trim += delta_t;
recalc_delta_settings(); 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; 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; LOOP_CAL_RAD(rad) z_pt[rad] -= z_pt[CEN] - z_center;
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(); recalc_delta_settings();
} }
static float auto_tune_h() { static float auto_tune_h(const float dcr) {
const float r_quot = dcr / delta_radius; const float r_quot = dcr / delta_radius;
return RECIPROCAL(r_quot / (2.0f / 3.0f)); // (2/3)/CR 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; constexpr float diff = 0.01f, delta_r = diff;
float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; float r_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 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 = -(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) r_fac = diff / r_fac / 3.0f; // 1/(3*delta_Z)
return r_fac; 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; constexpr float diff = 0.01f, delta_r = 0.0f;
float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f }; float a_fac = 0.0f, z_pt[NPP + 1] = { 0.0f };
abc_float_t delta_e = { 0.0f }, delta_t = { 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(); delta_t.reset();
LOOP_LINEAR_AXES(axis) { LOOP_LINEAR_AXES(axis) {
delta_t[axis] = diff; 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; 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) - _7P_STEP + NPP) % NPP + 1] / 6.0f;
a_fac -= z_pt[uint8_t((axis * _4P_STEP) + 1 + _7P_STEP)] / 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. * P3 Probe all positions: center, towers and opposite towers. Calibrate all.
* P4-P10 Probe all positions at different intermediate locations and average them. * 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 * T Don't calibrate tower angle corrections
* *
@ -386,7 +384,7 @@ static float auto_tune_a() {
* *
* E Engage the probe for each point * 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: * With SENSORLESS_PROBING:
* Use these flags to calibrate stall sensitivity: (e.g., `G33 P1 Y Z` to calibrate X only.) * 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; 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'); 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 #if HAS_PROBE_XY_OFFSET
// For offset probes the calibration radius is set to a safe but non-optimal value const float total_offset = HYPOT(probe.offset_xy.x, probe.offset_xy.y);
dcr -= HYPOT(probe.offset_xy.x, probe.offset_xy.y); dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset;
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.
#endif #endif
NOMORE(dcr, DELTA_PRINTABLE_RADIUS);
if (parser.seenval('R')) dcr = parser.value_float(); if (parser.seenval('R')) dcr -= _MAX(parser.value_float(),0);
if (!WITHIN(dcr, 0, max_dcr)) {
SERIAL_ECHOLNPGM("?calibration (R)adius implausible.");
return;
}
const float calibration_precision = parser.floatval('C', 0.0f); const float calibration_precision = parser.floatval('C', 0.0f);
if (calibration_precision < 0) { if (calibration_precision < 0) {
@ -475,8 +463,9 @@ void GcodeSuite::G33() {
SERIAL_ECHOLNPGM("G33 Auto Calibrate"); SERIAL_ECHOLNPGM("G33 Auto Calibrate");
// Report settings // Report settings
FSTR_P const checkingac = F("Checking... AC"); PGM_P const checkingac = PSTR("Checking... AC");
SERIAL_ECHOF(checkingac); SERIAL_ECHOPGM_P(checkingac);
SERIAL_ECHOPGM(" at radius:", dcr);
if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)"); if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
SERIAL_EOL(); SERIAL_EOL();
ui.set_status(checkingac); ui.set_status(checkingac);
@ -496,7 +485,7 @@ void GcodeSuite::G33() {
// Probe the points // Probe the points
zero_std_dev_old = zero_std_dev; 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"); SERIAL_ECHOLNPGM("Correct delta settings with M665 and M666");
return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index)); return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
} }
@ -536,10 +525,10 @@ void GcodeSuite::G33() {
// calculate factors // calculate factors
if (_7p_9_center) dcr *= 0.9f; if (_7p_9_center) dcr *= 0.9f;
h_factor = auto_tune_h(); h_factor = auto_tune_h(dcr);
r_factor = auto_tune_r(); r_factor = auto_tune_r(dcr);
a_factor = auto_tune_a(); a_factor = auto_tune_a(dcr);
dcr /= 0.9f; if (_7p_9_center) dcr /= 0.9f;
switch (probe_points) { switch (probe_points) {
case 0: case 0:

2
Marlin/src/inc/SanityCheck.h
View File

@ -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) #if ENABLED(NOZZLE_AS_PROBE)
static_assert(sanity_nozzle_to_probe_offset.x == 0 && sanity_nozzle_to_probe_offset.y == 0, 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."); "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 >= 0, "PROBING_MARGIN must be >= 0.");
static_assert(PROBING_MARGIN_BACK >= 0, "PROBING_MARGIN_BACK 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."); static_assert(PROBING_MARGIN_FRONT >= 0, "PROBING_MARGIN_FRONT must be >= 0.");

7
Marlin/src/module/probe.cpp
View File

@ -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 // 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) }; 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, probe_relative)) {
if (!can_reach(npos)) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable"); if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
return NAN; return NAN;
} }
npos -= offset_xy; // Get the nozzle position if (probe_relative) npos -= offset_xy; // Get the nozzle position
}
else if (!position_is_reachable(npos)) return NAN; // The given position is in terms of the nozzle
// Move the probe to the starting XYZ // Move the probe to the starting XYZ
do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S)); do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));

28
Marlin/src/module/probe.h
View File

@ -77,13 +77,20 @@ public:
#if HAS_PROBE_XY_OFFSET #if HAS_PROBE_XY_OFFSET
// Return true if the both nozzle and the probe can reach the given point. // 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. // 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) { 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? 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? && 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 #else
static bool can_reach(const_float_t rx, const_float_t ry) { static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) {
return position_is_reachable(rx, ry, PROBING_MARGIN); return position_is_reachable(rx, ry)
&& position_is_reachable(rx, ry, PROBING_MARGIN);
} }
#endif #endif
@ -96,11 +103,18 @@ public:
* Example: For a probe offset of -10,+10, then for the probe to reach 0,0 the * 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. * nozzle must be be able to reach +10,-10.
*/ */
static bool can_reach(const_float_t rx, const_float_t ry) { 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) return position_is_reachable(rx - offset_xy.x, ry - offset_xy.y)
&& COORDINATE_OKAY(rx, min_x() - fslop, max_x() + fslop) && COORDINATE_OKAY(rx, min_x() - fslop, max_x() + fslop)
&& COORDINATE_OKAY(ry, min_y() - fslop, max_y() + 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 #endif
@ -120,7 +134,7 @@ public:
static bool set_deployed(const bool) { return false; } 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 #endif
@ -132,7 +146,7 @@ public:
#endif #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) { static bool good_bounds(const xy_pos_t &lf, const xy_pos_t &rb) {
return ( return (