From 96b5da71983adffa86dffa5961da78aa24183fb4 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 25 Mar 2015 20:36:24 -0700 Subject: [PATCH] Fix up bed leveling code - Init `zprobe_zoffset` - Remove `current_position[Z_AXIS] = zprobe_zoffset` lines from the `set_bed_level_equation_*` functions - Apply standards to `mesh_bed_leveling` files - Document `MESH_BED_LEVELING` --- Marlin/ConfigurationStore.cpp | 2 +- Marlin/Marlin_main.cpp | 287 +++++++++++++++++----------------- Marlin/mesh_bed_leveling.cpp | 22 ++- Marlin/mesh_bed_leveling.h | 62 ++++---- 4 files changed, 181 insertions(+), 192 deletions(-) diff --git a/Marlin/ConfigurationStore.cpp b/Marlin/ConfigurationStore.cpp index fc485e2262..a0be202767 100644 --- a/Marlin/ConfigurationStore.cpp +++ b/Marlin/ConfigurationStore.cpp @@ -11,7 +11,7 @@ * max_acceleration_units_per_sq_second (x4) * acceleration * retract_acceleration - * travel_aceeleration + * travel_acceleration * minimumfeedrate * mintravelfeedrate * minsegmenttime diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index b3235f5595..ff6e964990 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -255,7 +255,7 @@ float home_offset[3] = { 0, 0, 0 }; float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS }; float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS }; bool axis_known_position[3] = { false, false, false }; -float zprobe_zoffset; +float zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER; // Extruder offset #if EXTRUDERS > 1 @@ -1092,9 +1092,6 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients) current_position[Y_AXIS] = corrected_position.y; current_position[Z_AXIS] = corrected_position.z; - // put the bed at 0 so we don't go below it. - current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); } #endif @@ -1121,9 +1118,6 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float current_position[Y_AXIS] = corrected_position.y; current_position[Z_AXIS] = corrected_position.z; - // put the bed at 0 so we don't go below it. - current_position[Z_AXIS] = zprobe_zoffset; - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); } @@ -2010,8 +2004,19 @@ inline void gcode_G28() { endstops_hit_on_purpose(); } -#if defined(MESH_BED_LEVELING) +#ifdef MESH_BED_LEVELING + /** + * G29: Mesh-based Z-Probe, probes a grid and produces a + * mesh to compensate for variable bed height + * + * Parameters With MESH_BED_LEVELING: + * + * S0 Produce a mesh report + * S1 Start probing mesh points + * S2 Probe the next mesh point + * + */ inline void gcode_G29() { static int probe_point = -1; int state = 0; @@ -2053,7 +2058,7 @@ inline void gcode_G28() { } else if (state == 2) { // Goto next point if (probe_point < 0) { - SERIAL_PROTOCOLPGM("Mesh probing not started.\n"); + SERIAL_PROTOCOLPGM("Start mesh probing with \"G29 S1\" first.\n"); return; } int ix, iy; @@ -2063,16 +2068,14 @@ inline void gcode_G28() { } else { ix = (probe_point-1) % MESH_NUM_X_POINTS; iy = (probe_point-1) / MESH_NUM_X_POINTS; - if (iy&1) { // Zig zag - ix = (MESH_NUM_X_POINTS - 1) - ix; - } + if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag mbl.set_z(ix, iy, current_position[Z_AXIS]); current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder); st_synchronize(); } - if (probe_point == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) { - SERIAL_PROTOCOLPGM("Mesh done.\n"); + if (probe_point == MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) { + SERIAL_PROTOCOLPGM("Mesh probing done.\n"); probe_point = -1; mbl.active = 1; enquecommands_P(PSTR("G28")); @@ -2080,9 +2083,7 @@ inline void gcode_G28() { } ix = probe_point % MESH_NUM_X_POINTS; iy = probe_point / MESH_NUM_X_POINTS; - if (iy&1) { // Zig zag - ix = (MESH_NUM_X_POINTS - 1) - ix; - } + if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag current_position[X_AXIS] = mbl.get_x(ix); current_position[Y_AXIS] = mbl.get_y(iy); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder); @@ -2091,9 +2092,7 @@ inline void gcode_G28() { } } -#endif - -#ifdef ENABLE_AUTO_BED_LEVELING +#elif defined(ENABLE_AUTO_BED_LEVELING) /** * G29: Detailed Z-Probe, probes the bed at 3 or more points. @@ -2154,9 +2153,9 @@ inline void gcode_G28() { #ifdef AUTO_BED_LEVELING_GRID - #ifndef DELTA - bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t'); - #endif + #ifndef DELTA + bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t'); + #endif if (verbose_level > 0) SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n"); @@ -2210,7 +2209,7 @@ inline void gcode_G28() { #ifdef Z_PROBE_SLED dock_sled(false); // engage (un-dock) the probe - #elif defined(Z_PROBE_ALLEN_KEY) + #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING) engage_z_probe(); #endif @@ -2218,19 +2217,18 @@ inline void gcode_G28() { #ifdef DELTA reset_bed_level(); - #else - - // make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly - //vector_3 corrected_position = plan_get_position_mm(); - //corrected_position.debug("position before G29"); - plan_bed_level_matrix.set_to_identity(); - vector_3 uncorrected_position = plan_get_position(); - //uncorrected_position.debug("position during G29"); - current_position[X_AXIS] = uncorrected_position.x; - current_position[Y_AXIS] = uncorrected_position.y; - current_position[Z_AXIS] = uncorrected_position.z; - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - #endif + #else //!DELTA + // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong + //vector_3 corrected_position = plan_get_position_mm(); + //corrected_position.debug("position before G29"); + plan_bed_level_matrix.set_to_identity(); + vector_3 uncorrected_position = plan_get_position(); + //uncorrected_position.debug("position during G29"); + current_position[X_AXIS] = uncorrected_position.x; + current_position[Y_AXIS] = uncorrected_position.y; + current_position[Z_AXIS] = uncorrected_position.z; + plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); + #endif //!DELTA setup_for_endstop_move(); @@ -2242,26 +2240,24 @@ inline void gcode_G28() { const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1); const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1); - #ifndef DELTA - // solve the plane equation ax + by + d = z - // A is the matrix with rows [x y 1] for all the probed points - // B is the vector of the Z positions - // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 - // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z - - int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points; - - double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations - eqnBVector[abl2], // "B" vector of Z points - mean = 0.0; - - #else - delta_grid_spacing[0] = xGridSpacing; - delta_grid_spacing[1] = yGridSpacing; - - float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER; - if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value(); - #endif + #ifdef DELTA + delta_grid_spacing[0] = xGridSpacing; + delta_grid_spacing[1] = yGridSpacing; + float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER; + if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value(); + #else // !DELTA + // solve the plane equation ax + by + d = z + // A is the matrix with rows [x y 1] for all the probed points + // B is the vector of the Z positions + // the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 + // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z + + int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points; + + double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations + eqnBVector[abl2], // "B" vector of Z points + mean = 0.0; + #endif // !DELTA int probePointCounter = 0; bool zig = true; @@ -2294,12 +2290,12 @@ inline void gcode_G28() { float measured_z, z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; - #ifdef DELTA - // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. - float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe); - if (distance_from_center > DELTA_PROBABLE_RADIUS) - continue; - #endif //DELTA + #ifdef DELTA + // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. + float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe); + if (distance_from_center > DELTA_PROBABLE_RADIUS) + continue; + #endif //DELTA // Enhanced G29 - Do not retract servo between probes ProbeAction act; @@ -2316,16 +2312,16 @@ inline void gcode_G28() { measured_z = probe_pt(xProbe, yProbe, z_before, act, verbose_level); - #ifndef DELTA - mean += measured_z; + #ifndef DELTA + mean += measured_z; - eqnBVector[probePointCounter] = measured_z; - eqnAMatrix[probePointCounter + 0 * abl2] = xProbe; - eqnAMatrix[probePointCounter + 1 * abl2] = yProbe; - eqnAMatrix[probePointCounter + 2 * abl2] = 1; - #else - bed_level[xCount][yCount] = measured_z + z_offset; - #endif + eqnBVector[probePointCounter] = measured_z; + eqnAMatrix[probePointCounter + 0 * abl2] = xProbe; + eqnAMatrix[probePointCounter + 1 * abl2] = yProbe; + eqnAMatrix[probePointCounter + 2 * abl2] = 1; + #else + bed_level[xCount][yCount] = measured_z + z_offset; + #endif probePointCounter++; } //xProbe @@ -2333,60 +2329,61 @@ inline void gcode_G28() { clean_up_after_endstop_move(); - #ifndef DELTA - // solve lsq problem - double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector); - - mean /= abl2; - - if (verbose_level) { - SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); - SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8); - SERIAL_PROTOCOLPGM(" b: "); - SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8); - SERIAL_PROTOCOLPGM(" d: "); - SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8); - SERIAL_EOL; - if (verbose_level > 2) { - SERIAL_PROTOCOLPGM("Mean of sampled points: "); - SERIAL_PROTOCOL_F(mean, 8); + #ifdef DELTA + extrapolate_unprobed_bed_level(); + print_bed_level(); + #else // !DELTA + // solve lsq problem + double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector); + + mean /= abl2; + + if (verbose_level) { + SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); + SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8); + SERIAL_PROTOCOLPGM(" b: "); + SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8); + SERIAL_PROTOCOLPGM(" d: "); + SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8); SERIAL_EOL; + if (verbose_level > 2) { + SERIAL_PROTOCOLPGM("Mean of sampled points: "); + SERIAL_PROTOCOL_F(mean, 8); + SERIAL_EOL; + } } - } - // Show the Topography map if enabled - if (do_topography_map) { - - SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n"); - SERIAL_PROTOCOLPGM("+-----------+\n"); - SERIAL_PROTOCOLPGM("|...Back....|\n"); - SERIAL_PROTOCOLPGM("|Left..Right|\n"); - SERIAL_PROTOCOLPGM("|...Front...|\n"); - SERIAL_PROTOCOLPGM("+-----------+\n"); - - for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) { - for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) { - int ind = yy * auto_bed_leveling_grid_points + xx; - float diff = eqnBVector[ind] - mean; - if (diff >= 0.0) - SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment - else - SERIAL_PROTOCOLPGM(" "); - SERIAL_PROTOCOL_F(diff, 5); - } // xx + // Show the Topography map if enabled + if (do_topography_map) { + + SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n"); + SERIAL_PROTOCOLPGM("+-----------+\n"); + SERIAL_PROTOCOLPGM("|...Back....|\n"); + SERIAL_PROTOCOLPGM("|Left..Right|\n"); + SERIAL_PROTOCOLPGM("|...Front...|\n"); + SERIAL_PROTOCOLPGM("+-----------+\n"); + + for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) { + for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) { + int ind = yy * auto_bed_leveling_grid_points + xx; + float diff = eqnBVector[ind] - mean; + if (diff >= 0.0) + SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment + else + SERIAL_PROTOCOLPGM(" "); + SERIAL_PROTOCOL_F(diff, 5); + } // xx + SERIAL_EOL; + } // yy SERIAL_EOL; - } // yy - SERIAL_EOL; - } //do_topography_map + } //do_topography_map - set_bed_level_equation_lsq(plane_equation_coefficients); - free(plane_equation_coefficients); - #else - extrapolate_unprobed_bed_level(); - print_bed_level(); - #endif + set_bed_level_equation_lsq(plane_equation_coefficients); + free(plane_equation_coefficients); + + #endif // !DELTA #else // !AUTO_BED_LEVELING_GRID @@ -2409,33 +2406,33 @@ inline void gcode_G28() { #endif // !AUTO_BED_LEVELING_GRID - #ifndef DELTA - if (verbose_level > 0) - plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:"); - - // Correct the Z height difference from z-probe position and hotend tip position. - // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. - // When the bed is uneven, this height must be corrected. - real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) - x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER; - y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER; - z_tmp = current_position[Z_AXIS]; - - apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset - current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. - plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); - #endif + #ifndef DELTA + if (verbose_level > 0) + plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:"); + + // Correct the Z height difference from z-probe position and hotend tip position. + // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. + // When the bed is uneven, this height must be corrected. + real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) + x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER; + y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER; + z_tmp = current_position[Z_AXIS]; + + apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset + current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. + plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); + #endif - #ifdef Z_PROBE_SLED - dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel - #elif defined(Z_PROBE_ALLEN_KEY) - retract_z_probe(); - #endif - - #ifdef Z_PROBE_END_SCRIPT - enquecommands_P(PSTR(Z_PROBE_END_SCRIPT)); - st_synchronize(); - #endif + #ifdef Z_PROBE_SLED + dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel + #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING) + retract_z_probe(); + #endif + + #ifdef Z_PROBE_END_SCRIPT + enquecommands_P(PSTR(Z_PROBE_END_SCRIPT)); + st_synchronize(); + #endif } #ifndef Z_PROBE_SLED diff --git a/Marlin/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp index b383fe589a..a48a6e6195 100644 --- a/Marlin/mesh_bed_leveling.cpp +++ b/Marlin/mesh_bed_leveling.cpp @@ -1,20 +1,16 @@ #include "mesh_bed_leveling.h" -#if defined(MESH_BED_LEVELING) +#ifdef MESH_BED_LEVELING -mesh_bed_leveling mbl; + mesh_bed_leveling mbl; -mesh_bed_leveling::mesh_bed_leveling() { - reset(); -} - -void mesh_bed_leveling::reset() { - for (int y=0; y get_x(i) && i < MESH_NUM_X_POINTS-1) { - i++; - } - return i-1; + int i = 1; + while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) i++; + return i - 1; } int select_y_index(float y) { - int i = 1; - while (y > get_y(i) && i < MESH_NUM_Y_POINTS-1) { - i++; - } - return i-1; + int i = 1; + while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++; + return i - 1; } float calc_z0(float a0, float a1, float z1, float a2, float z2) { - float delta_z = (z2 - z1)/(a2 - a1); - float delta_a = a0 - a1; - return z1 + delta_a * delta_z; + float delta_z = (z2 - z1)/(a2 - a1); + float delta_a = a0 - a1; + return z1 + delta_a * delta_z; } float get_z(float x0, float y0) { - int x_index = select_x_index(x0); - int y_index = select_y_index(y0); - float z1 = calc_z0(x0, - get_x(x_index), z_values[y_index][x_index], - get_x(x_index+1), z_values[y_index][x_index+1]); - float z2 = calc_z0(x0, - get_x(x_index), z_values[y_index+1][x_index], - get_x(x_index+1), z_values[y_index+1][x_index+1]); - float z0 = calc_z0(y0, - get_y(y_index), z1, - get_y(y_index+1), z2); - return z0; + int x_index = select_x_index(x0); + int y_index = select_y_index(y0); + float z1 = calc_z0(x0, + get_x(x_index), z_values[y_index][x_index], + get_x(x_index+1), z_values[y_index][x_index+1]); + float z2 = calc_z0(x0, + get_x(x_index), z_values[y_index+1][x_index], + get_x(x_index+1), z_values[y_index+1][x_index+1]); + float z0 = calc_z0(y0, + get_y(y_index), z1, + get_y(y_index+1), z2); + return z0; } -}; + }; -extern mesh_bed_leveling mbl; + extern mesh_bed_leveling mbl; #endif // MESH_BED_LEVELING