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
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 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: