Implemented linear interpolation for automatic bed leveling Z adjustm…

…ent.

Marlin/Configuration.h

@@ -303,21 +303,21 @@ const bool Z_ENDSTOPS_INVERTING = false; // set to true to invert the logic of t
 #define NUM_AXIS 4 // The axis order in all axis related arrays is X, Y, Z, E
 #define HOMING_FEEDRATE {200*60, 200*60, 200*60, 0}  // set the homing speeds (mm/min)
 
-#define Z_PROBE_OFFSET {0, -13.6, -6.47, 0}  // Distance between hotend nozzle and deployed bed leveling probe.
+#define Z_PROBE_OFFSET {0, -13.6, -6.4, 0}  // Distance between hotend nozzle and deployed bed leveling probe.
 
 // default settings 
 
 #define DEFAULT_AXIS_STEPS_PER_UNIT   {100, 100, 100, 1250}
-#define DEFAULT_MAX_FEEDRATE          {320, 320, 320, 25}  // (mm/sec)
-#define DEFAULT_MAX_ACCELERATION      {2000, 2000, 2000, 2000}    // X, Y, Z, E maximum start speed for accelerated moves.
+#define DEFAULT_MAX_FEEDRATE          {320, 320, 320, 20}  // (mm/sec)
+#define DEFAULT_MAX_ACCELERATION      {2000, 2000, 2000, 200}    // X, Y, Z, E maximum start speed for accelerated moves.
 
 #define DEFAULT_ACCELERATION          2000   // X, Y, Z and E max acceleration in mm/s^2 for printing moves
 #define DEFAULT_RETRACT_ACCELERATION  2000   // X, Y, Z and E max acceleration in mm/s^2 for r retracts
 
 // 
 #define DEFAULT_XYJERK                20.0   // (mm/sec)
 #define DEFAULT_ZJERK                 20.0   // (mm/sec)
-#define DEFAULT_EJERK                 20.0   // (mm/sec)
+#define DEFAULT_EJERK                 1.0   // (mm/sec)
 
 //===========================================================================
 //=============================Additional Features===========================

Marlin/Marlin_main.cpp

@@ -672,49 +672,49 @@ void retract_z_probe() {
   feedrate = 400*60;
   destination[X_AXIS] = -40;
   destination[Y_AXIS] = -82;
-  destination[Z_AXIS] = 10;
   prepare_move_raw();
 
+  // Move the nozzle below the print surface to push the probe up.
   feedrate = 20*60;
-  destination[Z_AXIS] = -5;
+  destination[Z_AXIS] = current_position[Z_AXIS] - 14;
   prepare_move_raw();
 
   feedrate = 400*60;
-  destination[Z_AXIS] = 10;
+  destination[Z_AXIS] = current_position[Z_AXIS] + 30;
   prepare_move_raw();
   // Try again because sometimes the last move doesn't flush properly.
-  destination[Z_AXIS] = 10.1;
+  destination[Z_AXIS] = current_position[Z_AXIS] + 0.1;
   prepare_move_raw();
   st_synchronize();
 }
 
 float z_probe() {
   feedrate = 400*60;
-  destination[Z_AXIS] = 10;
   prepare_move_raw();
   st_synchronize();
 
   enable_endstops(true);
+  float start_z = current_position[Z_AXIS];
   long start_steps = st_get_position(Z_AXIS);
 
   feedrate = 50*60;
-  destination[Z_AXIS] = 0;
+  destination[Z_AXIS] = -20;
   prepare_move_raw();
   st_synchronize();
   endstops_hit_on_purpose();
 
   enable_endstops(false);
   long stop_steps = st_get_position(Z_AXIS);
 
-  float mm = 10.0 - float(start_steps - stop_steps)
-    / axis_steps_per_unit[Z_AXIS];
+  float mm = start_z -
+    float(start_steps - stop_steps) / axis_steps_per_unit[Z_AXIS];
   current_position[Z_AXIS] = mm;
   calculate_delta(current_position);
   plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS],
 		    current_position[E_AXIS]);
 
   feedrate = 400*60;
-  destination[Z_AXIS] = 10;
+  destination[Z_AXIS] = mm+2;
   prepare_move_raw();
   return mm;
 }
@@ -731,6 +731,16 @@ void calibrate_print_surface(float z_offset) {
         bed_level[x+3][y+3] = 0.0;
       }
     }
+    // For unprobed positions just copy nearest neighbor.
+    if (abs(y) >= 3) {
+      bed_level[1][y+3] = bed_level[2][y+3];
+      bed_level[5][y+3] = bed_level[4][y+3];
+    }
+    if (abs(y) >=2) {
+      bed_level[0][y+3] = bed_level[1][y+3];
+      bed_level[6][y+3] = bed_level[5][y+3];
+    }
+    // Print calibration results for manual frame adjustment.
     for (int x = -3; x <= 3; x++) {
       SERIAL_PROTOCOL_F(bed_level[x+3][y+3], 3);
       SERIAL_PROTOCOLPGM(" ");
@@ -1817,11 +1827,22 @@ void calculate_delta(float cartesian[3])
   */
 }
 
+// Adjust print surface height by linear interpolation over the bed_level array.
 void adjust_delta(float cartesian[3])
 {
-  int grid_x = max(0, min(7, int(cartesian[X_AXIS]/25.0 + 3.5)));
-  int grid_y = max(0, min(7, int(cartesian[Y_AXIS]/25.0 + 3.5)));
-  float offset = bed_level[grid_x][grid_y];
+  float grid_x = max(-2.999, min(2.999, cartesian[X_AXIS]/25.0));
+  float grid_y = max(-2.999, min(2.999, cartesian[Y_AXIS]/25.0));
+  int floor_x = floor(grid_x);
+  int floor_y = floor(grid_y);
+  float ratio_x = grid_x - floor_x;
+  float ratio_y = grid_y - floor_y;
+  float z1 = bed_level[floor_x+3][floor_y+3];
+  float z2 = bed_level[floor_x+3][floor_y+4];
+  float z3 = bed_level[floor_x+4][floor_y+3];
+  float z4 = bed_level[floor_x+4][floor_y+4];
+  float left = (1-ratio_y)*z1 + ratio_y*z2;
+  float right = (1-ratio_y)*z3 + ratio_y*z4;
+  float offset = (1-ratio_x)*left + ratio_x*right;
 
   delta[X_AXIS] += offset;
   delta[Y_AXIS] += offset;
@@ -1830,6 +1851,16 @@ void adjust_delta(float cartesian[3])
   /*
   SERIAL_ECHOPGM("grid_x="); SERIAL_ECHO(grid_x);
   SERIAL_ECHOPGM(" grid_y="); SERIAL_ECHO(grid_y);
+  SERIAL_ECHOPGM(" floor_x="); SERIAL_ECHO(floor_x);
+  SERIAL_ECHOPGM(" floor_y="); SERIAL_ECHO(floor_y);
+  SERIAL_ECHOPGM(" ratio_x="); SERIAL_ECHO(ratio_x);
+  SERIAL_ECHOPGM(" ratio_y="); SERIAL_ECHO(ratio_y);
+  SERIAL_ECHOPGM(" z1="); SERIAL_ECHO(z1);
+  SERIAL_ECHOPGM(" z2="); SERIAL_ECHO(z2);
+  SERIAL_ECHOPGM(" z3="); SERIAL_ECHO(z3);
+  SERIAL_ECHOPGM(" z4="); SERIAL_ECHO(z4);
+  SERIAL_ECHOPGM(" left="); SERIAL_ECHO(left);
+  SERIAL_ECHOPGM(" right="); SERIAL_ECHO(right);
   SERIAL_ECHOPGM(" offset="); SERIAL_ECHOLN(offset);
   */
 }

Marlin/pins.h

@@ -952,7 +952,7 @@
 #define HEATER_1_PIN       -1
 #define HEATER_2_PIN       -1
 #define HEATER_BED_PIN     20  // Bed
-#define FAN_PIN            22  // Fan
+#define FAN_PIN            16  // Fan
 // You may need to change FAN_PIN to 16 because Marlin isn't using fastio.h
 // for the fan and Teensyduino uses a different pin mapping.