From 26a9cd0b082e2bcccd7b54c36eec657f0ecc8b96 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 21 Jun 2016 01:25:23 -0700 Subject: [PATCH] Move run_z_probe to earlier block --- Marlin/Marlin_main.cpp | 158 +++++++++++++++++++---------------------- 1 file changed, 75 insertions(+), 83 deletions(-) diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 794aec5a8..fe8559dd8 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -1960,89 +1960,6 @@ static void setup_for_endstop_move() { endstops.enable_z_probe(false); } -#endif // HAS_BED_PROBE - -#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 ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position); - #endif - - sync_plan_position(); - } - - #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(); - - 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(); - - if (planeNormal.z < 0) { - planeNormal.x = -planeNormal.x; - planeNormal.y = -planeNormal.y; - planeNormal.z = -planeNormal.z; - } - - planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); - - vector_3 corrected_position = planner.adjusted_position(); - - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) { - vector_3 uncorrected_position = corrected_position; - DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position); - } - #endif - - current_position[X_AXIS] = corrected_position.x; - current_position[Y_AXIS] = corrected_position.y; - current_position[Z_AXIS] = corrected_position.z; - - #if ENABLED(DEBUG_LEVELING_FEATURE) - if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position); - #endif - - sync_plan_position(); - } - - #endif // !AUTO_BED_LEVELING_GRID - -#endif // AUTO_BED_LEVELING_FEATURE - -#if HAS_BED_PROBE - static void run_z_probe() { float old_feedrate = feedrate; @@ -2134,6 +2051,81 @@ static void setup_for_endstop_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 ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position); + #endif + + sync_plan_position(); + } + + #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(); + + 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(); + + if (planeNormal.z < 0) { + planeNormal.x = -planeNormal.x; + planeNormal.y = -planeNormal.y; + planeNormal.z = -planeNormal.z; + } + + planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal); + + vector_3 corrected_position = planner.adjusted_position(); + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) { + vector_3 uncorrected_position = corrected_position; + DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position); + } + #endif + + current_position[X_AXIS] = corrected_position.x; + current_position[Y_AXIS] = corrected_position.y; + current_position[Z_AXIS] = corrected_position.z; + + #if ENABLED(DEBUG_LEVELING_FEATURE) + if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position); + #endif + + sync_plan_position(); + } + + #endif // !AUTO_BED_LEVELING_GRID + inline void do_blocking_move_to_xy(float x, float y) { do_blocking_move_to(x, y, current_position[Z_AXIS]); }