andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

vanilla_fb_2.0.x
Luc Van Daele 3 years ago
committed by Scott Lahteine
parent
39a81d167e
commit
656034d2d9
5 changed files with 66 additions and 66 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 71
      Marlin/src/gcode/calibrate/G33.cpp
  2. 2
      Marlin/src/inc/SanityCheck.h
  3. 7
      Marlin/src/module/probe.cpp
  4. 38
      Marlin/src/module/probe.h

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 dcr;
void ac_home() {
endstops.enable(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) {
#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
UNUSED(stow);
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
*/
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,
_1p_calibration = probe_points == 1 || probe_points == -1,
_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
* - 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};
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;
#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);
}
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];
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_tower_angle_trim += delta_t;
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;
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;
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();
}
static float auto_tune_h() {
static float auto_tune_h(const float dcr) {
const float r_quot = dcr / delta_radius;
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;
float r_fac = 0.0f, z_pt[NPP + 1] = { 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 = diff / r_fac / 3.0f; // 1/(3*delta_Z)
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;
float a_fac = 0.0f, z_pt[NPP + 1] = { 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();
LOOP_LINEAR_AXES(axis) {
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;
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;
@ -370,7 +368,7 @@ static float auto_tune_a() {
* P3 Probe all positions: center, towers and opposite towers. Calibrate all.
* 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
*
@ -386,7 +384,7 @@ static float auto_tune_a() {
*
* 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:
* 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;
}
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');
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
// For offset probes the calibration radius is set to a safe but non-optimal value
dcr -= HYPOT(probe.offset_xy.x, probe.offset_xy.y);
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.
const float total_offset = HYPOT(probe.offset_xy.x, probe.offset_xy.y);
dcr -= probe_at_offset ? _MAX(total_offset, PROBING_MARGIN) : total_offset;
#endif
if (parser.seenval('R')) dcr = parser.value_float();
if (!WITHIN(dcr, 0, max_dcr)) {
SERIAL_ECHOLNPGM("?calibration (R)adius implausible.");
return;
}
NOMORE(dcr, DELTA_PRINTABLE_RADIUS);
if (parser.seenval('R')) dcr -= _MAX(parser.value_float(),0);
const float calibration_precision = parser.floatval('C', 0.0f);
if (calibration_precision < 0) {
@ -475,8 +463,9 @@ void GcodeSuite::G33() {
SERIAL_ECHOLNPGM("G33 Auto Calibrate");
// Report settings
FSTR_P const checkingac = F("Checking... AC");
SERIAL_ECHOF(checkingac);
PGM_P const checkingac = PSTR("Checking... AC");
SERIAL_ECHOPGM_P(checkingac);
SERIAL_ECHOPGM(" at radius:", dcr);
if (verbose_level == 0) SERIAL_ECHOPGM(" (DRY-RUN)");
SERIAL_EOL();
ui.set_status(checkingac);
@ -496,7 +485,7 @@ void GcodeSuite::G33() {
// Probe the points
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");
return ac_cleanup(TERN_(HAS_MULTI_HOTEND, old_tool_index));
}
@ -536,10 +525,10 @@ void GcodeSuite::G33() {
// calculate factors
if (_7p_9_center) dcr *= 0.9f;
h_factor = auto_tune_h();
r_factor = auto_tune_r();
a_factor = auto_tune_a();
dcr /= 0.9f;
h_factor = auto_tune_h(dcr);
r_factor = auto_tune_r(dcr);
a_factor = auto_tune_a(dcr);
if (_7p_9_center) dcr /= 0.9f;
switch (probe_points) {
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)
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.");
#else
#elif !IS_KINEMATIC
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_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
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)) {
if (!can_reach(npos, probe_relative)) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
return NAN;
}
npos -= offset_xy; // Get the nozzle position
}
else if (!position_is_reachable(npos)) return NAN; // The given position is in terms of the nozzle
if (probe_relative) npos -= offset_xy; // Get the nozzle position
// Move the probe to the starting XYZ
do_blocking_move_to(npos, feedRate_t(XY_PROBE_FEEDRATE_MM_S));

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

@ -77,13 +77,20 @@ public:
#if HAS_PROBE_XY_OFFSET
// 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.
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?
&& 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
static bool can_reach(const_float_t rx, const_float_t ry) {
return position_is_reachable(rx, ry, PROBING_MARGIN);
static bool can_reach(const_float_t rx, const_float_t ry, const bool=true) {
return position_is_reachable(rx, ry)
&& position_is_reachable(rx, ry, PROBING_MARGIN);
}
#endif
@ -96,11 +103,18 @@ public:
* 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.
*/
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)
&& COORDINATE_OKAY(rx, min_x() - fslop, max_x() + 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
@ -120,7 +134,7 @@ public:
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
@ -132,7 +146,7 @@ public:
#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) {
return (
@ -161,30 +175,30 @@ public:
TERN_(DELTA, DELTA_PRINTABLE_RADIUS)
TERN_(IS_SCARA, SCARA_PRINTABLE_RADIUS)
);
static constexpr float probe_radius(const xy_pos_t &probe_offset_xy = offset_xy) {
static constexpr float probe_radius(const xy_pos_t &probe_offset_xy=offset_xy) {
return printable_radius - _MAX(PROBING_MARGIN, HYPOT(probe_offset_xy.x, probe_offset_xy.y));
}
#endif
static constexpr float _min_x(const xy_pos_t &probe_offset_xy = offset_xy) {
static constexpr float _min_x(const xy_pos_t &probe_offset_xy=offset_xy) {
return TERN(IS_KINEMATIC,
(X_CENTER) - probe_radius(probe_offset_xy),
_MAX((X_MIN_BED) + (PROBING_MARGIN_LEFT), (X_MIN_POS) + probe_offset_xy.x)
);
}
static constexpr float _max_x(const xy_pos_t &probe_offset_xy = offset_xy) {
static constexpr float _max_x(const xy_pos_t &probe_offset_xy=offset_xy) {
return TERN(IS_KINEMATIC,
(X_CENTER) + probe_radius(probe_offset_xy),
_MIN((X_MAX_BED) - (PROBING_MARGIN_RIGHT), (X_MAX_POS) + probe_offset_xy.x)
);
}
static constexpr float _min_y(const xy_pos_t &probe_offset_xy = offset_xy) {
static constexpr float _min_y(const xy_pos_t &probe_offset_xy=offset_xy) {
return TERN(IS_KINEMATIC,
(Y_CENTER) - probe_radius(probe_offset_xy),
_MAX((Y_MIN_BED) + (PROBING_MARGIN_FRONT), (Y_MIN_POS) + probe_offset_xy.y)
);
}
static constexpr float _max_y(const xy_pos_t &probe_offset_xy = offset_xy) {
static constexpr float _max_y(const xy_pos_t &probe_offset_xy=offset_xy) {
return TERN(IS_KINEMATIC,
(Y_CENTER) + probe_radius(probe_offset_xy),
_MIN((Y_MAX_BED) - (PROBING_MARGIN_BACK), (Y_MAX_POS) + probe_offset_xy.y)

Write Preview
Loading…
Cancel
Save