MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases 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

2
Marlin/src/HAL/LPC1768/fast_pwm.cpp
View File

@ -26,7 +26,7 @@
void set_pwm_duty(const pin_t pin, const uint16_t v, const uint16_t v_size/*=255*/, const bool invert/*=false*/) {
if (!LPC176x::pin_is_valid(pin)) return;
if (LPC176x::pwm_attach_pin(pin))
if (LPC176x::pwm_attach_pin(pin))
LPC176x::pwm_write_ratio(pin, invert ? 1.0f - (float)v / v_size : (float)v / v_size); // map 1-254 onto PWM range
}

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.");

11
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 (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
return NAN;
}
npos -= offset_xy; // Get the nozzle position
if (!can_reach(npos, probe_relative)) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Position Not Reachable");
return NAN;
}
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));

46
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) {
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?
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, 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,10 +103,17 @@ 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) {
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);
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)