melch/Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Move run_z_probe to earlier block

Browse Source
This commit is contained in:
Scott Lahteine 2016-06-21 01:25:23 -07:00
parent 7f8a2025af
commit 26a9cd0b08
1 changed files with 75 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
Marlin/Marlin_main.cpp
View File

@ -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]);
}