Improvement in the mesh bed leveling routine: Z-lift before XY to home.3.0.6-082216

Fix of the print stop: Reset mesh bed leveling and baby stepping
on print stop.

4 files changed, 35 insertions, 24 deletions

diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp
index 0eedde5fb..27a4a8d68 100644
--- a/Firmware/Marlin_main.cpp
+++ b/Firmware/Marlin_main.cpp
@@ -1389,13 +1389,13 @@ inline void set_current_to_destination() { memcpy(current_position, destination,
 inline void set_destination_to_current() { memcpy(destination, current_position, sizeof(destination)); }
 
 
-static void setup_for_endstop_move() {
+static void setup_for_endstop_move(bool enable_endstops_now = true) {
     saved_feedrate = feedrate;
     saved_feedmultiply = feedmultiply;
     feedmultiply = 100;
     previous_millis_cmd = millis();
     
-    enable_endstops(true);
+    enable_endstops(enable_endstops_now);
 }
 
 static void clean_up_after_endstop_move() {
@@ -2408,7 +2408,7 @@ void process_commands()
             int Z_PROBE_FEEDRATE = homing_feedrate[Z_AXIS]/60;
             int Z_LIFT_FEEDRATE = homing_feedrate[Z_AXIS]/40;
             bool has_z = is_bed_z_jitter_data_valid();
-            setup_for_endstop_move();
+            setup_for_endstop_move(false);
             const char *kill_message = NULL;
             while (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
                 // Get coords of a measuring point.
@@ -2428,16 +2428,15 @@ void process_commands()
                     #endif
                 }
             
-                // Move Z to proper distance
+                // Move Z up to MESH_HOME_Z_SEARCH.
                 current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
                 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
                 st_synchronize();
 
+                // Move to XY position of the sensor point.
                 current_position[X_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point);
                 current_position[Y_AXIS] = pgm_read_float(bed_ref_points+2*mesh_point+1);
                 world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-//                mbl.get_meas_xy(ix, iy, current_position[X_AXIS], current_position[Y_AXIS], false);
-                enable_endstops(false);
                 plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
                 st_synchronize();
                 
@@ -2464,8 +2463,8 @@ void process_commands()
             }
             current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
             plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS],current_position[Z_AXIS] , current_position[E_AXIS], Z_LIFT_FEEDRATE, active_extruder);
+            st_synchronize();
             if (mesh_point != MESH_MEAS_NUM_X_POINTS * MESH_MEAS_NUM_Y_POINTS) {
-                st_synchronize();
                 kill(kill_message);
             }
             clean_up_after_endstop_move();
@@ -2525,12 +2524,7 @@ void process_commands()
 
             mbl.upsample_3x3();
             mbl.active = 1;
-            current_position[X_AXIS] = X_MIN_POS+0.2;
-            current_position[Y_AXIS] = Y_MIN_POS+0.2;
-            current_position[Z_AXIS] = Z_MIN_POS;
-            world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);
-            plan_buffer_line(current_position[X_AXIS], current_position[X_AXIS], current_position[Z_AXIS], current_position[E_AXIS], XY_AXIS_FEEDRATE, active_extruder);
-            st_synchronize();
+            go_home_with_z_lift();
 
             // Restore custom message state
             custom_message       = custom_message_old;
diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp
index 4177dc93d..a4bc9fc7d 100644
--- a/Firmware/mesh_bed_calibration.cpp
+++ b/Firmware/mesh_bed_calibration.cpp
@@ -1932,6 +1932,25 @@ canceled:
     return result;

 }

 

+void go_home_with_z_lift()

+{

+    // Don't let the manage_inactivity() function remove power from the motors.

+    refresh_cmd_timeout();

+    // Go home.

+    // First move up to a safe height.

+    current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;

+    go_to_current(homing_feedrate[Z_AXIS]/60);

+    // Second move to XY [0, 0].

+    current_position[X_AXIS] = X_MIN_POS+0.2;

+    current_position[Y_AXIS] = Y_MIN_POS+0.2;

+    // Clamp to the physical coordinates.

+    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);

+    go_to_current(homing_feedrate[X_AXIS]/60);

+    // Third move up to a safe height.

+    current_position[Z_AXIS] = Z_MIN_POS;

+    go_to_current(homing_feedrate[Z_AXIS]/60);    

+}

+

 // Sample the 9 points of the bed and store them into the EEPROM as a reference.

 // When calling this function, the X, Y, Z axes should be already homed,

 // and the world2machine correction matrix should be active.

@@ -2025,17 +2044,7 @@ bool sample_mesh_and_store_reference()
     mbl.upsample_3x3();

     mbl.active = true;

 

-    // Don't let the manage_inactivity() function remove power from the motors.

-    refresh_cmd_timeout();

-

-    // Go home.

-    current_position[Z_AXIS] = Z_MIN_POS;

-    go_to_current(homing_feedrate[Z_AXIS]/60);

-    current_position[X_AXIS] = X_MIN_POS+0.2;

-    current_position[Y_AXIS] = Y_MIN_POS+0.2;

-    // Clamp to the physical coordinates.

-    world2machine_clamp(current_position[X_AXIS], current_position[Y_AXIS]);

-    go_to_current(homing_feedrate[X_AXIS]/60);

+    go_home_with_z_lift();

 

     enable_endstops(endstops_enabled);

     enable_z_endstop(endstop_z_enabled);

diff --git a/Firmware/mesh_bed_calibration.h b/Firmware/mesh_bed_calibration.h
index f1f8ff7ae..102fa48dd 100644
--- a/Firmware/mesh_bed_calibration.h
+++ b/Firmware/mesh_bed_calibration.h
@@ -142,6 +142,7 @@ inline bool world2machine_clamp(float &x, float &y)
 

 extern bool find_bed_induction_sensor_point_z(float minimum_z = -10.f, uint8_t n_iter = 3);

 extern bool find_bed_induction_sensor_point_xy();

+extern void go_home_with_z_lift();

 

 // Positive or zero: ok

 // Negative: failed

diff --git a/Firmware/ultralcd.cpp b/Firmware/ultralcd.cpp
index 2629073c0..8c6375dda 100644
--- a/Firmware/ultralcd.cpp
+++ b/Firmware/ultralcd.cpp
@@ -10,6 +10,7 @@
 #include <string.h>
 
 #include "util.h"
+#include "mesh_bed_leveling.h"
 //#include "Configuration.h"
 
 
@@ -2665,8 +2666,14 @@ void lcd_sdcard_stop()
 		if ((int32_t)encoderPosition == 2)
 		{
 				cancel_heatup = true;
+        #ifdef MESH_BED_LEVELING
+        mbl.active = false;
+        #endif
         // Stop the stoppers, update the position from the stoppers.
         planner_abort_hard();
+        // Because the planner_abort_hard() initialized current_position[Z] from the stepper,
+        // Z baystep is no more applied. Reset it.
+        babystep_reset();
         // Clean the input command queue.
         cmdqueue_reset();
 				lcd_setstatuspgm(MSG_PRINT_ABORTED);