Move run_z_probe to earlier block
This commit is contained in:
Scott Lahteine
parent
7f8a2025af
commit
26a9cd0b08
|
|
@ -1960,89 +1960,6 @@ static void setup_for_endstop_move() {
|
||||||
endstops.enable_z_probe(false);
|
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() {
|
static void run_z_probe() {
|
||||||
|
|
||||||
float old_feedrate = feedrate;
|
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_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) {
|
inline void do_blocking_move_to_xy(float x, float y) {
|
||||||
do_blocking_move_to(x, y, current_position[Z_AXIS]);
|
do_blocking_move_to(x, y, current_position[Z_AXIS]);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in a new issue