From 9248cfb0ad7a26ef44bb92a01bc7fe7bf749c4be Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 21 Aug 2016 03:10:55 -0500 Subject: [PATCH] Bed leveling that accounts for home XYZ --- Marlin/Conditionals_post.h | 5 + Marlin/Marlin_main.cpp | 284 ++++++++++++++++++------------------- Marlin/planner.cpp | 119 ++++++++-------- Marlin/planner.h | 9 +- 4 files changed, 210 insertions(+), 207 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 6eab11dac..239780f34 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -165,6 +165,11 @@ #ifndef Z_SAFE_HOMING_Y_POINT #define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2) #endif + #define X_TILT_FULCRUM Z_SAFE_HOMING_X_POINT + #define Y_TILT_FULCRUM Z_SAFE_HOMING_Y_POINT + #else + #define X_TILT_FULCRUM X_HOME_POS + #define Y_TILT_FULCRUM Y_HOME_POS #endif /** diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 7ab7af853..4ea114504 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -458,45 +458,51 @@ static uint8_t target_extruder; #if ENABLED(DELTA) - #define TOWER_1 X_AXIS - #define TOWER_2 Y_AXIS - #define TOWER_3 Z_AXIS - - float delta[ABC]; - float cartesian_position[XYZ] = { 0 }; #define SIN_60 0.8660254037844386 #define COS_60 0.5 - float endstop_adj[ABC] = { 0 }; + + float delta[ABC], + cartesian_position[XYZ] = { 0 }, + endstop_adj[ABC] = { 0 }; + // these are the default values, can be overriden with M665 - float delta_radius = DELTA_RADIUS; - float delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower - float delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); - float delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); // front right tower - float delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); - float delta_tower3_x = 0; // back middle tower - float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3); - float delta_diagonal_rod = DELTA_DIAGONAL_ROD; - float delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1; - float delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2; - float delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3; - float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1); - float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2); - float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3); - float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND; - float delta_clip_start_height = Z_MAX_POS; + float delta_radius = DELTA_RADIUS, + delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower + delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), + delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower + delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), + delta_tower3_x = 0, // back middle tower + delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3), + delta_diagonal_rod = DELTA_DIAGONAL_ROD, + delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1, + delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2, + delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3, + delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1), + delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2), + delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3), + delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND, + delta_clip_start_height = Z_MAX_POS; + #if ENABLED(AUTO_BED_LEVELING_FEATURE) int delta_grid_spacing[2] = { 0, 0 }; float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS]; #endif + float delta_safe_distance_from_top(); + void set_cartesian_from_steppers(); + #else + static bool home_all_axis = true; + #endif #if ENABLED(SCARA) - float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND; - float delta[ABC]; - float axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1 + float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND, + delta[ABC], + axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1 + cartesian_position[XYZ] = { 0 }; + void set_cartesian_from_steppers() { } // to be written later #endif #if ENABLED(FILAMENT_WIDTH_SENSOR) @@ -2266,79 +2272,37 @@ static void clean_up_after_endstop_or_probe_move() { #if ENABLED(AUTO_BED_LEVELING_FEATURE) - #if ENABLED(AUTO_BED_LEVELING_GRID) - - #if DISABLED(DELTA) - - static void set_bed_level_equation_lsq(double* plane_equation_coefficients) { - - //planner.bed_level_matrix.debug("bed level before"); - - #if ENABLED(DEBUG_LEVELING_FEATURE) - planner.bed_level_matrix.set_to_identity(); - if (DEBUGGING(LEVELING)) { - vector_3 uncorrected_position = planner.adjusted_position(); - DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position); - DEBUG_POS(">>> set_bed_level_equation_lsq", current_position); - } - #endif - - vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); - planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); - - vector_3 corrected_position = planner.adjusted_position(); - current_position[X_AXIS] = corrected_position.x; - current_position[Y_AXIS] = corrected_position.y; - current_position[Z_AXIS] = corrected_position.z; + #if DISABLED(DELTA) - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position); - #endif - - SYNC_PLAN_POSITION_KINEMATIC(); - } - - #endif // !DELTA - - #else // !AUTO_BED_LEVELING_GRID - - static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) { - - planner.bed_level_matrix.set_to_identity(); + /** + * Get the stepper positions, apply the rotation matrix + * using the home XY and Z0 position as the fulcrum. + */ + vector_3 untilted_stepper_position() { + vector_3 pos = vector_3( + RAW_X_POSITION(stepper.get_axis_position_mm(X_AXIS)) - X_TILT_FULCRUM, + RAW_Y_POSITION(stepper.get_axis_position_mm(Y_AXIS)) - Y_TILT_FULCRUM, + RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS)) + ); - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - vector_3 uncorrected_position = planner.adjusted_position(); - DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position); - } - #endif + matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix); - vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1); - vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2); - vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3); - vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal(); + //pos.debug("untilted_stepper_position offset"); + //bed_level_matrix.debug("untilted_stepper_position"); + //inverse.debug("in untilted_stepper_position"); - if (planeNormal.z < 0) { - planeNormal.x = -planeNormal.x; - planeNormal.y = -planeNormal.y; - planeNormal.z = -planeNormal.z; - } + pos.apply_rotation(inverse); - planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); - vector_3 corrected_position = planner.adjusted_position(); + pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM); + pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM); + pos.z = LOGICAL_Z_POSITION(pos.z); - current_position[X_AXIS] = corrected_position.x; - current_position[Y_AXIS] = corrected_position.y; - current_position[Z_AXIS] = corrected_position.z; + //pos.debug("after rotation and reorientation"); - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position); - #endif - - SYNC_PLAN_POSITION_KINEMATIC(); + return pos; } - #endif // !AUTO_BED_LEVELING_GRID + #endif // !DELTA #if ENABLED(DELTA) @@ -3626,41 +3590,41 @@ inline void gcode_G28() { #endif // AUTO_BED_LEVELING_GRID - if (!dryrun) { + stepper.synchronize(); - #if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA) - if (DEBUGGING(LEVELING)) { - vector_3 corrected_position = planner.adjusted_position(); - DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position); - DEBUG_POS("BEFORE matrix.set_to_identity", current_position); - } - #endif + if (!dryrun) { - // make sure the bed_level_rotation_matrix is identity or the planner will get it wrong + // Reset the bed_level_matrix because leveling + // needs to be done without leveling enabled. planner.bed_level_matrix.set_to_identity(); - #if ENABLED(DELTA) - reset_bed_level(); - #else //!DELTA - - //vector_3 corrected_position = planner.adjusted_position(); - //corrected_position.debug("position before G29"); - vector_3 uncorrected_position = planner.adjusted_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; + // + // Re-orient the current position without leveling + // based on where the steppers are positioned. + // + #if ENABLED(DELTA) || ENABLED(SCARA) - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position); + #if ENABLED(DELTA) + reset_bed_level(); #endif - SYNC_PLAN_POSITION_KINEMATIC(); + // For DELTA/SCARA we need to apply forward kinematics. + // This returns raw positions and we remap to the space. + set_cartesian_from_steppers(); + LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i); + + #else + + // For cartesian/core the steppers are already mapped to + // the coordinate space by design. + LOOP_XYZ(i) current_position[i] = stepper.get_axis_position_mm((AxisEnum)i); #endif // !DELTA - } - stepper.synchronize(); + // Inform the planner about the new coordinates + // (This is probably not needed here) + SYNC_PLAN_POSITION_KINEMATIC(); + } setup_for_endstop_or_probe_move(); @@ -3766,7 +3730,20 @@ inline void gcode_G28() { LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y), stow_probe_after_each, verbose_level); - if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); + if (!dryrun) { + vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1), + pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2), + pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3); + + vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal(); + + if (planeNormal.z < 0) { + planeNormal.x *= -1; + planeNormal.y *= -1; + planeNormal.z *= -1; + } + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + } #endif // !AUTO_BED_LEVELING_GRID @@ -3810,7 +3787,12 @@ inline void gcode_G28() { } } - if (!dryrun) set_bed_level_equation_lsq(plane_equation_coefficients); + // Create the matrix but don't correct the position yet + if (!dryrun) { + planner.bed_level_matrix = matrix_3x3::create_look_at( + vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1) + ); + } // Show the Topography map if enabled if (do_topography_map) { @@ -3851,6 +3833,7 @@ inline void gcode_G28() { SERIAL_EOL; } // yy SERIAL_EOL; + if (verbose_level > 3) { SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:"); @@ -3876,47 +3859,60 @@ inline void gcode_G28() { SERIAL_EOL; } } //do_topography_map + #endif //!DELTA + #endif // AUTO_BED_LEVELING_GRID #if DISABLED(DELTA) + if (verbose_level > 0) planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); if (!dryrun) { - /** - * Correct the Z height difference from Z probe position and nozzle tip position. - * The Z height on homing is measured by Z probe, but the Z probe is quite far - * from the nozzle. When the bed is uneven, this height must be corrected. - */ - float 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], - stepper_z = stepper.get_axis_position_mm(Z_AXIS); //get the real Z (since planner.adjusted_position is now correcting the plane) + // + // Correct the current XYZ position based on the tilted plane. + // + + // Get the distance from the reference point to the current position + // The current XY is in sync with the planner/steppers at this point + // but the current Z is only known to the steppers. + float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM, + y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM, + z_real = RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS)); #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) { - SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > stepper_z = ", stepper_z); - SERIAL_ECHOLNPAIR(" ... z_tmp = ", z_tmp); + SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist); + SERIAL_ECHOPAIR("y_dist:", y_dist); + SERIAL_ECHOPAIR("z_real:", z_real); } #endif - // Apply the correction sending the Z probe offset - apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp); + // Apply the matrix to the distance from the reference point to XY, + // and from the homed Z to the current Z. + apply_rotation_xyz(planner.bed_level_matrix, x_dist, y_dist, z_real); #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) - SERIAL_ECHOLNPAIR("> AFTER apply_rotation_xyz > z_tmp = ", z_tmp); + if (DEBUGGING(LEVELING)) { + SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist); + SERIAL_ECHOPAIR("y_dist:", y_dist); + SERIAL_ECHOPAIR("z_real:", z_real); + } #endif - // Adjust the current Z and send it to the planner. - current_position[Z_AXIS] += z_tmp - stepper_z; + // Apply the rotated distance and Z to the current position + current_position[X_AXIS] = LOGICAL_X_POSITION(X_TILT_FULCRUM + x_dist); + current_position[Y_AXIS] = LOGICAL_Y_POSITION(Y_TILT_FULCRUM + y_dist); + current_position[Z_AXIS] = LOGICAL_Z_POSITION(z_real); + SYNC_PLAN_POSITION_KINEMATIC(); #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected Z in G29", current_position); + if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected XYZ in G29", current_position); #endif } + #endif // !DELTA #ifdef Z_PROBE_END_SCRIPT @@ -7850,15 +7846,15 @@ void ok_to_send() { RAW_Z_POSITION(in_cartesian[Z_AXIS]) }; - delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1 + delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1 - sq(delta_tower1_x - cartesian[X_AXIS]) - sq(delta_tower1_y - cartesian[Y_AXIS]) ) + cartesian[Z_AXIS]; - delta[TOWER_2] = sqrt(delta_diagonal_rod_2_tower_2 + delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2 - sq(delta_tower2_x - cartesian[X_AXIS]) - sq(delta_tower2_y - cartesian[Y_AXIS]) ) + cartesian[Z_AXIS]; - delta[TOWER_3] = sqrt(delta_diagonal_rod_2_tower_3 + delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3 - sq(delta_tower3_x - cartesian[X_AXIS]) - sq(delta_tower3_y - cartesian[Y_AXIS]) ) + cartesian[Z_AXIS]; @@ -7867,9 +7863,9 @@ void ok_to_send() { SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); - SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[TOWER_1]); - SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[TOWER_2]); - SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[TOWER_3]); + SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]); + SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]); + SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]); */ } @@ -7880,10 +7876,10 @@ void ok_to_send() { LOGICAL_Z_POSITION(0) }; inverse_kinematics(cartesian); - float distance = delta[TOWER_3]; + float distance = delta[A_AXIS]; cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS); inverse_kinematics(cartesian); - return abs(distance - delta[TOWER_3]); + return abs(distance - delta[A_AXIS]); } void forward_kinematics_DELTA(float z1, float z2, float z3) { @@ -8014,7 +8010,7 @@ void set_current_from_steppers_for_axis(AxisEnum axis) { set_cartesian_from_steppers(); current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis); #elif ENABLED(AUTO_BED_LEVELING_FEATURE) - vector_3 pos = planner.adjusted_position(); + vector_3 pos = untilted_stepper_position(); current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z; #else current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 066c1445b..bcad2c906 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -521,6 +521,38 @@ void Planner::check_axes_activity() { #endif } +#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) + + void Planner::apply_leveling( + #if ENABLED(MESH_BED_LEVELING) + const float &x, const float &y + #else + float &x, float &y + #endif + , float &z + ) { + #if ENABLED(MESH_BED_LEVELING) + + if (mbl.active()) + z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y)); + + #elif ENABLED(AUTO_BED_LEVELING_FEATURE) + + float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM), + ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM), + tz = RAW_Z_POSITION(z); + + apply_rotation_xyz(bed_level_matrix, tx, ty, tz); + + x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM); + y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM); + z = LOGICAL_Z_POSITION(tz); + + #endif + } + +#endif + /** * Planner::buffer_line * @@ -531,12 +563,14 @@ void Planner::check_axes_activity() { * extruder - target extruder */ -#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) - void Planner::buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder) -#else - void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder) -#endif // AUTO_BED_LEVELING_FEATURE -{ +void Planner::buffer_line( + #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) + float x, float y, float z + #else + const float& x, const float& y, const float& z + #endif + , const float& e, float fr_mm_s, const uint8_t extruder +) { // Calculate the buffer head after we push this byte int next_buffer_head = next_block_index(block_buffer_head); @@ -544,11 +578,8 @@ void Planner::check_axes_activity() { // Rest here until there is room in the buffer. while (block_buffer_tail == next_buffer_head) idle(); - #if ENABLED(MESH_BED_LEVELING) - if (mbl.active()) - z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]); - #elif ENABLED(AUTO_BED_LEVELING_FEATURE) - apply_rotation_xyz(bed_level_matrix, x, y, z); + #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE) + apply_leveling(x, y, z); #endif // The target position of the tool in absolute steps @@ -1116,61 +1147,33 @@ void Planner::check_axes_activity() { } // buffer_line() -#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA) - - /** - * Get the XYZ position of the steppers as a vector_3. - * - * On CORE machines XYZ is derived from ABC. - */ - vector_3 Planner::adjusted_position() { - vector_3 pos = vector_3(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS)); - - //pos.debug("in Planner::adjusted_position"); - //bed_level_matrix.debug("in Planner::adjusted_position"); - - matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix); - //inverse.debug("in Planner::inverse"); - - pos.apply_rotation(inverse); - //pos.debug("after rotation"); - - return pos; - } - -#endif // AUTO_BED_LEVELING_FEATURE && !DELTA - /** * Directly set the planner XYZ position (hence the stepper positions). * * On CORE machines stepper ABC will be translated from the given XYZ. */ -#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) - void Planner::set_position_mm(float x, float y, float z, const float& e) -#else - void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e) -#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING - { - #if ENABLED(MESH_BED_LEVELING) - - if (mbl.active()) - z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y)); - - #elif ENABLED(AUTO_BED_LEVELING_FEATURE) - - apply_rotation_xyz(bed_level_matrix, x, y, z); +void Planner::set_position_mm( + #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) + float x, float y, float z + #else + const float& x, const float& y, const float& z + #endif + , const float& e +) { - #endif + #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE) + apply_leveling(x, y, z); + #endif - long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]), - ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]), - nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]), - ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]); - stepper.set_position(nx, ny, nz, ne); - previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. + long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]), + ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]), + nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]), + ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]); + stepper.set_position(nx, ny, nz, ne); + previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. - LOOP_XYZE(i) previous_speed[i] = 0.0; - } + LOOP_XYZE(i) previous_speed[i] = 0.0; +} /** * Directly set the planner E position (hence the stepper E position). diff --git a/Marlin/planner.h b/Marlin/planner.h index e1159294a..ecca0fd3f 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -203,11 +203,10 @@ class Planner { #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) - #if ENABLED(AUTO_BED_LEVELING_FEATURE) - /** - * The corrected position, applying the bed level matrix - */ - static vector_3 adjusted_position(); + #if ENABLED(MESH_BED_LEVELING) + static void apply_leveling(const float &x, const float &y, float &z); + #else + static void apply_leveling(float &x, float &y, float &z); #endif /**