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

leaner code for probe routine

Browse Source
This commit is contained in:
LVD-AC 2017-04-30 17:19:18 +02:00 committed by teemuatlut
parent 585c00a728
commit 471a321624
1 changed files with 89 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
Marlin/Marlin_main.cpp
View File

@ -61,7 +61,7 @@
* G30 - Single Z probe, probes bed at X Y location (defaults to current XY location) * G30 - Single Z probe, probes bed at X Y location (defaults to current XY location)
* G31 - Dock sled (Z_PROBE_SLED only) * G31 - Dock sled (Z_PROBE_SLED only)
* G32 - Undock sled (Z_PROBE_SLED only) * G32 - Undock sled (Z_PROBE_SLED only)
* G33 - Delta '1-4-7-point' auto calibration : "G33 P<points> <A> <O> <T> V<verbose>" (Requires DELTA) * G33 - Delta '1-4-7-point' auto calibration : "G33 V<verbose> P<points> <A> <O> <T>" (Requires DELTA)
* G38 - Probe target - similar to G28 except it uses the Z_MIN_PROBE for all three axes * G38 - Probe target - similar to G28 except it uses the Z_MIN_PROBE for all three axes
* G90 - Use Absolute Coordinates * G90 - Use Absolute Coordinates
* G91 - Use Relative Coordinates * G91 - Use Relative Coordinates
@ -4994,8 +4994,12 @@ inline void gcode_G28() {
* G33 - Delta '1-4-7-point' auto calibration (Requires DELTA) * G33 - Delta '1-4-7-point' auto calibration (Requires DELTA)
* *
* Usage: * Usage:
* G33 <Pn> <A> <O> <T> <Vn> * G33 <Vn> <Pn> <A> <O> <T>
* *
* Vn = verbose level (n=0-2 default 1)
* n=0 dry-run mode: setting + probe results / no calibration
* n=1 settings
* n=2 setting + probe results
* Pn = n=-7 -> +7 : n*n probe points * Pn = n=-7 -> +7 : n*n probe points
* calibrates height ('1 point'), endstops, and delta radius ('4 points') * calibrates height ('1 point'), endstops, and delta radius ('4 points')
* and tower angles with n > 2 ('7+ points') * and tower angles with n > 2 ('7+ points')
@ -5006,10 +5010,6 @@ inline void gcode_G28() {
* A = abort 1 point delta height calibration after 1 probe * A = abort 1 point delta height calibration after 1 probe
* O = use oposite tower points instead of tower points with 4 point calibration * O = use oposite tower points instead of tower points with 4 point calibration
* T = do not calibrate tower angles with 7+ point calibration * T = do not calibrate tower angles with 7+ point calibration
* Vn = verbose level (n=0-2 default 1)
* n=0 dry-run mode: no calibration
* n=1 settings
* n=2 setting + probe results
*/ */
inline void gcode_G33() { inline void gcode_G33() {
@ -5019,14 +5019,14 @@ inline void gcode_G28() {
set_bed_leveling_enabled(false); set_bed_leveling_enabled(false);
#endif #endif
int8_t pp = code_seen('P') ? code_value_int() : DELTA_CALIBRATION_DEFAULT_POINTS, int8_t pp = (code_seen('P') ? code_value_int() : DELTA_CALIBRATION_DEFAULT_POINTS),
probe_mode = (WITHIN(pp, 1, 7)) ? pp : DELTA_CALIBRATION_DEFAULT_POINTS; probe_mode = (WITHIN(pp, 1, 7) ? pp : DELTA_CALIBRATION_DEFAULT_POINTS);
probe_mode = (code_seen('A') && probe_mode == 1) ? -probe_mode : probe_mode; probe_mode = (code_seen('A') && probe_mode == 1 ? -probe_mode : probe_mode);
probe_mode = (code_seen('O') && probe_mode == 2) ? -probe_mode : probe_mode; probe_mode = (code_seen('O') && probe_mode == 2 ? -probe_mode : probe_mode);
probe_mode = (code_seen('T') && probe_mode > 2) ? -probe_mode : probe_mode; probe_mode = (code_seen('T') && probe_mode > 2 ? -probe_mode : probe_mode);
int8_t verbose_level = code_seen('V') ? code_value_byte() : 1; int8_t verbose_level = (code_seen('V') ? code_value_byte() : 1);
if (!WITHIN(verbose_level, 0, 2)) verbose_level = 1; if (!WITHIN(verbose_level, 0, 2)) verbose_level = 1;
@ -5034,7 +5034,7 @@ inline void gcode_G28() {
const static char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h"; const static char save_message[] PROGMEM = "Save with M500 and/or copy to Configuration.h";
float test_precision, float test_precision,
zero_std_dev = verbose_level ? 999.0 : 0.0, // 0.0 in dry-run mode : forced end zero_std_dev = (verbose_level ? 999.0 : 0.0), // 0.0 in dry-run mode : forced end
e_old[XYZ] = { e_old[XYZ] = {
endstop_adj[A_AXIS], endstop_adj[A_AXIS],
endstop_adj[B_AXIS], endstop_adj[B_AXIS],
@ -5046,11 +5046,17 @@ inline void gcode_G28() {
beta_old = delta_tower_angle_trim[B_AXIS]; beta_old = delta_tower_angle_trim[B_AXIS];
int8_t iterations = 0, int8_t iterations = 0,
probe_points = abs(probe_mode); probe_points = abs(probe_mode);
bool _1_point = (probe_points <= 1), const bool pp_equals_1 = (probe_points == 1),
_7_point = (probe_mode > 2), pp_equals_2 = (probe_points == 2),
o_mode = (probe_mode == -2), pp_equals_3 = (probe_points == 3),
towers = (probe_points > 2 || probe_mode == 2), pp_equals_4 = (probe_points == 4),
opposites = (probe_points > 2 || o_mode); pp_equals_5 = (probe_points == 5),
pp_equals_6 = (probe_points == 6),
pp_equals_7 = (probe_points == 7),
pp_greather_2 = (probe_points > 2),
pp_greather_3 = (probe_points > 3),
pp_greather_4 = (probe_points > 4),
pp_greather_5 = (probe_points > 5);
// print settings // print settings
@ -5061,7 +5067,7 @@ inline void gcode_G28() {
LCD_MESSAGEPGM("Checking... AC"); LCD_MESSAGEPGM("Checking... AC");
SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]); SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
if (!_1_point) { if (!pp_equals_1) {
SERIAL_PROTOCOLPGM(" Ex:"); SERIAL_PROTOCOLPGM(" Ex:");
if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+'); if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2); SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2);
@ -5074,7 +5080,7 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPAIR(" Radius:", delta_radius); SERIAL_PROTOCOLPAIR(" Radius:", delta_radius);
} }
SERIAL_EOL; SERIAL_EOL;
if (_7_point) { if (probe_mode > 2) { // negative disables tower angles
SERIAL_PROTOCOLPGM(".Tower angle : Tx:"); SERIAL_PROTOCOLPGM(".Tower angle : Tx:");
if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+'); if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2); SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2);
@ -5092,78 +5098,69 @@ inline void gcode_G28() {
do { do {
float z_at_pt[13] = { 0 }, float z_at_pt[13] = { 0 },
S1 = z_at_pt[0], S1 = 0.0,
S2 = sq(S1); S2 = 0.0;
int16_t N = 1; int16_t N = 0;
bool _4_probe = (probe_points == 2),
_7_probe = (probe_points > 2),
center_probe = (probe_points != 3 && probe_points != 6),
multi_circle = (probe_points > 4),
diff_circle = (probe_points > 5),
max_circle = (probe_points > 6),
intermediates = (probe_points == 4 || diff_circle);
setup_for_endstop_or_probe_move();
test_precision = zero_std_dev; test_precision = zero_std_dev;
iterations++; iterations++;
// probe the points // probe the points
int16_t center_points = 0; if (!pp_equals_3 && !pp_equals_6) { // probe the centre
setup_for_endstop_or_probe_move();
if (center_probe) { // probe centre
z_at_pt[0] += probe_pt(0.0, 0.0 , true, 1); z_at_pt[0] += probe_pt(0.0, 0.0 , true, 1);
center_points = 1; clean_up_after_endstop_or_probe_move();
} }
if (pp_greather_2) { // probe extra centre points
int16_t step_axis = (multi_circle) ? 2 : 4, for (int8_t axis = (pp_greather_4 ? 11 : 9); axis > 0; axis -= (pp_greather_4 ? 2 : 4)) {
start = (multi_circle) ? 11 : 9; setup_for_endstop_or_probe_move();
if (_7_probe) { // probe extra 3 or 6 centre points
for (int8_t axis = start; axis > 0; axis -= step_axis) {
z_at_pt[0] += probe_pt( z_at_pt[0] += probe_pt(
cos(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius), cos(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius),
sin(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius), true, 1); sin(RADIANS(180 + 30 * axis)) * (0.1 * delta_calibration_radius), true, 1);
clean_up_after_endstop_or_probe_move();
} }
center_points += (multi_circle) ? 6 : 3; // average centre points z_at_pt[0] /= (pp_equals_5 ? 7 : probe_points);
z_at_pt[0] /= center_points;
} }
if (!pp_equals_1) { // probe the radius
start = (o_mode) ? 3 : 1; float start_circles = (pp_equals_7 ? -1.5 : pp_equals_6 || pp_equals_5 ? -1 : 0),
step_axis = (_4_probe) ? 4 : (intermediates) ? 1 : 2;
if (!_1_point) {
float start_circles = (max_circle) ? -1.5 : (multi_circle) ? -1 : 0, // one or multi radius points
end_circles = -start_circles; end_circles = -start_circles;
bool zig_zag = true; bool zig_zag = true;
for (uint8_t axis = start; axis < 13; axis += step_axis) { // probes 3, 6 or 12 points on the calibration radius for (uint8_t axis = (probe_mode == -2 ? 3 : 1); axis < 13;
for (float circles = start_circles ; circles <= end_circles; circles++) // one or multi radius points axis += (pp_equals_2 ? 4 : pp_equals_3 || pp_equals_5 ? 2 : 1)) {
for (float circles = start_circles ; circles <= end_circles; circles++) {
setup_for_endstop_or_probe_move();
z_at_pt[axis] += probe_pt( z_at_pt[axis] += probe_pt(
cos(RADIANS(180 + 30 * axis)) * (1 + circles * 0.1 * ((zig_zag) ? 1 : -1)) * delta_calibration_radius, cos(RADIANS(180 + 30 * axis)) *
sin(RADIANS(180 + 30 * axis)) * (1 + circles * 0.1 * ((zig_zag) ? 1 : -1)) * delta_calibration_radius, true, 1); (1 + circles * 0.1 * (zig_zag ? 1 : -1)) * delta_calibration_radius,
sin(RADIANS(180 + 30 * axis)) *
if (diff_circle) { (1 + circles * 0.1 * (zig_zag ? 1 : -1)) * delta_calibration_radius, true, 1);
start_circles += (zig_zag) ? 0.5 : -0.5; // opposites: one radius point less clean_up_after_endstop_or_probe_move();
}
start_circles += (pp_greather_5 ? (zig_zag ? 0.5 : -0.5) : 0);
end_circles = -start_circles; end_circles = -start_circles;
}
zig_zag = !zig_zag; zig_zag = !zig_zag;
if (multi_circle) z_at_pt[axis] /= (zig_zag) ? 3.0 : 2.0; // average between radius points z_at_pt[axis] /= (pp_equals_7 ? (zig_zag ? 4.0 : 3.0) :
pp_equals_6 ? (zig_zag ? 3.0 : 2.0) : pp_equals_5 ? 3 : 1);
} }
} }
if (intermediates) step_axis = 2; if (pp_greather_3 && !pp_equals_5) // average intermediates to tower and opposites
for (uint8_t axis = 1; axis < 13; axis += 2)
for (uint8_t axis = start; axis < 13; axis += step_axis) { // average half intermediates to towers and opposites
if (intermediates)
z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0; z_at_pt[axis] = (z_at_pt[axis] + (z_at_pt[axis + 1] + z_at_pt[(axis + 10) % 12 + 1]) / 2.0) / 2.0;
S1 += z_at_pt[0];
S2 += sq(z_at_pt[0]);
N++;
if (!pp_equals_1) // std dev from zero plane
for (uint8_t axis = 1; axis < 13; axis += (pp_equals_2 ? 4 : 2)) {
S1 += z_at_pt[axis]; S1 += z_at_pt[axis];
S2 += sq(z_at_pt[axis]); S2 += sq(z_at_pt[axis]);
N++; N++;
} }
zero_std_dev = round(sqrt(S2 / N) * 1000.0) / 1000.0 + 0.00001;
// Solve matrices // Solve matrices
zero_std_dev = round(sqrt(S2 / N) * 1000.0) / 1000.0 + 0.00001; // deviation from zero plane
if (zero_std_dev < test_precision) { if (zero_std_dev < test_precision) {
COPY(e_old, endstop_adj); COPY(e_old, endstop_adj);
dr_old = delta_radius; dr_old = delta_radius;
@ -5173,7 +5170,6 @@ inline void gcode_G28() {
float e_delta[XYZ] = { 0.0 }, r_delta = 0.0, float e_delta[XYZ] = { 0.0 }, r_delta = 0.0,
t_alpha = 0.0, t_beta = 0.0; t_alpha = 0.0, t_beta = 0.0;
const float r_diff = delta_radius - delta_calibration_radius, const float r_diff = delta_radius - delta_calibration_radius,
h_factor = 1.00 + r_diff * 0.001, //1.02 for r_diff = 20mm h_factor = 1.00 + r_diff * 0.001, //1.02 for r_diff = 20mm
r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)), //2.25 for r_diff = 20mm r_factor = -(1.75 + 0.005 * r_diff + 0.001 * sq(r_diff)), //2.25 for r_diff = 20mm
@ -5218,43 +5214,42 @@ inline void gcode_G28() {
e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3); e_delta[Z_AXIS] = Z1050(0) - Z0175(1) - Z0175(5) + Z0350(9) + Z0175(7) + Z0175(11) - Z0350(3);
r_delta = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3); r_delta = Z2250(0) - Z0375(1) - Z0375(5) - Z0375(9) - Z0375(7) - Z0375(11) - Z0375(3);
if (probe_mode > 0) { //probe points negative disables tower angles if (probe_mode > 0) { // negative disables tower angles
t_alpha = + Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3); t_alpha = + Z0444(1) - Z0888(5) + Z0444(9) + Z0444(7) - Z0888(11) + Z0444(3);
t_beta = - Z0888(1) + Z0444(5) + Z0444(9) - Z0888(7) + Z0444(11) + Z0444(3); t_beta = - Z0888(1) + Z0444(5) + Z0444(9) - Z0888(7) + Z0444(11) + Z0444(3);
} }
break; break;
} }
// adjust delta_height and endstops by the max amount
LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis]; LOOP_XYZ(axis) endstop_adj[axis] += e_delta[axis];
delta_radius += r_delta; delta_radius += r_delta;
delta_tower_angle_trim[A_AXIS] += t_alpha;
delta_tower_angle_trim[B_AXIS] -= t_beta;
// adjust delta_height and endstops by the max amount
const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]); const float z_temp = MAX3(endstop_adj[A_AXIS], endstop_adj[B_AXIS], endstop_adj[C_AXIS]);
home_offset[Z_AXIS] -= z_temp; home_offset[Z_AXIS] -= z_temp;
LOOP_XYZ(i) endstop_adj[i] -= z_temp; LOOP_XYZ(i) endstop_adj[i] -= z_temp;
delta_tower_angle_trim[A_AXIS] += t_alpha;
delta_tower_angle_trim[B_AXIS] -= t_beta;
recalc_delta_settings(delta_radius, delta_diagonal_rod); recalc_delta_settings(delta_radius, delta_diagonal_rod);
} }
else { // !iterate else { // step one back
// step one back
COPY(endstop_adj, e_old); COPY(endstop_adj, e_old);
delta_radius = dr_old; delta_radius = dr_old;
home_offset[Z_AXIS] = zh_old; home_offset[Z_AXIS] = zh_old;
delta_tower_angle_trim[A_AXIS] = alpha_old; delta_tower_angle_trim[A_AXIS] = alpha_old;
delta_tower_angle_trim[B_AXIS] = beta_old; delta_tower_angle_trim[B_AXIS] = beta_old;
recalc_delta_settings(delta_radius, delta_diagonal_rod); recalc_delta_settings(delta_radius, delta_diagonal_rod);
} }
// print report // print report
if (verbose_level == 2) { if (verbose_level != 1) {
SERIAL_PROTOCOLPGM(". c:"); SERIAL_PROTOCOLPGM(". c:");
if (z_at_pt[0] > 0) SERIAL_CHAR('+'); if (z_at_pt[0] > 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(z_at_pt[0], 2); SERIAL_PROTOCOL_F(z_at_pt[0], 2);
if (towers) { if (probe_mode == 2 || pp_greather_2) {
SERIAL_PROTOCOLPGM(" x:"); SERIAL_PROTOCOLPGM(" x:");
if (z_at_pt[1] >= 0) SERIAL_CHAR('+'); if (z_at_pt[1] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(z_at_pt[1], 2); SERIAL_PROTOCOL_F(z_at_pt[1], 2);
@ -5265,11 +5260,11 @@ inline void gcode_G28() {
if (z_at_pt[9] >= 0) SERIAL_CHAR('+'); if (z_at_pt[9] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(z_at_pt[9], 2); SERIAL_PROTOCOL_F(z_at_pt[9], 2);
} }
if (!o_mode) SERIAL_EOL; if (probe_mode != -2) SERIAL_EOL;
if (opposites) { if (probe_mode == -2 || pp_greather_2) {
if (_7_probe) { if (pp_greather_2) {
SERIAL_CHAR('.'); SERIAL_CHAR('.');
SERIAL_PROTOCOL_SP(12); SERIAL_PROTOCOL_SP(13);
} }
SERIAL_PROTOCOLPGM(" yz:"); SERIAL_PROTOCOLPGM(" yz:");
if (z_at_pt[7] >= 0) SERIAL_CHAR('+'); if (z_at_pt[7] >= 0) SERIAL_CHAR('+');
@ -5303,7 +5298,7 @@ inline void gcode_G28() {
lcd_setstatus(mess); lcd_setstatus(mess);
} }
SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]); SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
if (!_1_point) { if (!pp_equals_1) {
SERIAL_PROTOCOLPGM(" Ex:"); SERIAL_PROTOCOLPGM(" Ex:");
if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+'); if (endstop_adj[A_AXIS] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2); SERIAL_PROTOCOL_F(endstop_adj[A_AXIS], 2);
@ -5316,7 +5311,7 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPAIR(" Radius:", delta_radius); SERIAL_PROTOCOLPAIR(" Radius:", delta_radius);
} }
SERIAL_EOL; SERIAL_EOL;
if (_7_point) { if (probe_mode > 2) { // negative disables tower angles
SERIAL_PROTOCOLPGM(".Tower angle : Tx:"); SERIAL_PROTOCOLPGM(".Tower angle : Tx:");
if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+'); if (delta_tower_angle_trim[A_AXIS] >= 0) SERIAL_CHAR('+');
SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2); SERIAL_PROTOCOL_F(delta_tower_angle_trim[A_AXIS], 2);
@ -5328,6 +5323,7 @@ inline void gcode_G28() {
} }
if (zero_std_dev >= test_precision) if (zero_std_dev >= test_precision)
serialprintPGM(save_message); serialprintPGM(save_message);
SERIAL_EOL;
} }
else { // forced end else { // forced end
if (verbose_level == 0) { if (verbose_level == 0) {
@ -5343,10 +5339,10 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]); SERIAL_PROTOCOLPAIR(".Height:", DELTA_HEIGHT + home_offset[Z_AXIS]);
SERIAL_EOL; SERIAL_EOL;
serialprintPGM(save_message); serialprintPGM(save_message);
SERIAL_EOL;
} }
} }
clean_up_after_endstop_or_probe_move();
stepper.synchronize(); stepper.synchronize();
gcode_G28(); gcode_G28();