WIP. Adding bed leveling code.

Marlin/Configuration.h

 //#define ENDSTOPPULLUP_FIL_RUNOUT // Uncomment to use internal pullup for filament runout pins if the sensor is defined.
 
 //===========================================================================
+//============================ Manual Bed Leveling ==========================
+//===========================================================================
+
+#define MANUAL_BED_LEVELING  // Add display menu option for bed leveling
+#define MESH_BED_LEVELING    // Enable mesh bed leveling
+
+#if defined(MESH_BED_LEVELING)
+  #define MESH_MIN_X 10
+  #define MESH_MAX_X (X_MAX_POS - MESH_MIN_X)
+  #define MESH_MIN_Y 10
+  #define MESH_MAX_Y (Y_MAX_POS - MESH_MIN_Y)
+  #define MESH_NUM_X_POINTS 4
+  #define MESH_NUM_Y_POINTS 3
+#endif  // MESH_BED_LEVELING
+
+//===========================================================================
 //============================= Bed Auto Leveling ===========================
 //===========================================================================
 

Marlin/Marlin_main.cpp

 
 #define SERVO_LEVELING defined(ENABLE_AUTO_BED_LEVELING) && PROBE_SERVO_DEACTIVATION_DELAY > 0
 
+#if defined(MESH_BED_LEVELING)
+  #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 #include "ultralcd.h"
 #include "planner.h"
 #include "stepper.h"
 }
 #endif
 
+#if defined(MESH_BED_LEVELING)
+#if !defined(MIN)
+#define MIN(_v1, _v2) (((_v1) < (_v2)) ? (_v1) : (_v2))
+#endif  // ! MIN
+// This function is used to split lines on mesh borders so each segment is only part of one mesh area
+void mesh_plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder, uint8_t x_splits=0xff, uint8_t y_splits=0xff)
+{
+  int pix = mbl.select_x_index(current_position[X_AXIS]);
+  int piy = mbl.select_y_index(current_position[Y_AXIS]);
+  int ix = mbl.select_x_index(x);
+  int iy = mbl.select_y_index(y);
+  pix = MIN(pix, MESH_NUM_X_POINTS-2);
+  piy = MIN(piy, MESH_NUM_Y_POINTS-2);
+  ix = MIN(ix, MESH_NUM_X_POINTS-2);
+  iy = MIN(iy, MESH_NUM_Y_POINTS-2);
+  if (ix > pix && (x_splits)&(1<<ix)) {
+    float nx = mbl.get_x(ix);
+    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
+    float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    x_splits ^= 1 << ix;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (ix < pix && (x_splits)&(1<<pix)) {
+    float nx = mbl.get_x(pix);
+    float normalized_dist = (nx - current_position[X_AXIS])/(x - current_position[X_AXIS]);
+    float ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
+    float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    x_splits ^= 1 << pix;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (iy > piy && (y_splits)&(1<<iy)) {
+    float ny = mbl.get_y(iy);
+    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
+    float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    y_splits ^= 1 << iy;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  } else if (iy < piy && (y_splits)&(1<<piy)) {
+    float ny = mbl.get_y(piy);
+    float normalized_dist = (ny - current_position[Y_AXIS])/(y - current_position[Y_AXIS]);
+    float nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
+    float ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
+    y_splits ^= 1 << piy;
+    mesh_plan_buffer_line(nx, ny, z, ne, feed_rate, extruder, x_splits, y_splits);
+    mesh_plan_buffer_line(x, y, z, e, feed_rate, extruder, x_splits, y_splits);
+    return;
+  }
+  plan_buffer_line(x, y, z, e, feedrate, extruder);
+  for(int8_t i=0; i < NUM_AXIS; i++) {
+    current_position[i] = destination[i];
+  }
+}
+#endif  // MESH_BED_LEVELING
+
 void prepare_move()
 {
   clamp_to_software_endstops(destination);
 #if ! (defined DELTA || defined SCARA)
   // Do not use feedmultiply for E or Z only moves
   if( (current_position[X_AXIS] == destination [X_AXIS]) && (current_position[Y_AXIS] == destination [Y_AXIS])) {
-      plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
-  }
-  else {
+    plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
+  } else {
+#if defined(MESH_BED_LEVELING)
+    mesh_plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+    return;
+#else
     plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate*feedmultiply/60/100.0, active_extruder);
+#endif  // MESH_BED_LEVELING
   }
 #endif // !(DELTA || SCARA)
 

Marlin/mesh_bed_leveling.cpp

+#include "mesh_bed_leveling.h"
+
+#if defined(MESH_BED_LEVELING)
+
+mesh_bed_leveling mbl;
+
+#endif  // MESH_BED_LEVELING

Marlin/mesh_bed_leveling.h

+#include "Marlin.h"
+
+#if defined(MESH_BED_LEVELING)
+
+#define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
+#define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
+
+class mesh_bed_leveling {
+public:
+    
+    float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
+    
+    mesh_bed_leveling() {
+        reset();
+    }
+    
+    void reset() {
+        for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
+            for (int x=0; x<MESH_NUM_X_POINTS; x++) {
+                z_values[y][x] = 0;
+            }
+        }
+    }
+    
+    float get_x(int i) { return MESH_MIN_X + MESH_X_DIST*i; }
+    float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST*i; }
+    void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
+    
+    int select_x_index(float x) {
+        int i = 1;
+        while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) {
+            i++;
+        }
+        return i-1;
+    }
+    
+    int select_y_index(float y) {
+        int i = 1;
+        while (y > get_y(i) && i < MESH_NUM_Y_POINTS-1) {
+            i++;
+        }
+        return i-1;
+    }
+    
+    float calc_z0(float a0, float a1, float z1, float a2, float z2) {
+        float delta_z = (z2 - z1)/(a2 - a1);
+        float delta_a = a0 - a1;
+        return z1 + delta_a * delta_z;
+    }
+    
+    float get_z(float x0, float y0) {
+        int x_index = select_x_index(x0);
+        int y_index = select_y_index(y0);
+        float z1 = calc_z0(x0,
+                           get_x(x_index), z_values[y_index][x_index],
+                           get_x(x_index+1), z_values[y_index][x_index+1]);
+        float z2 = calc_z0(x0,
+                           get_x(x_index), z_values[y_index+1][x_index],
+                           get_x(x_index+1), z_values[y_index+1][x_index+1]);
+        float z0 = calc_z0(y0,
+                           get_y(y_index), z1,
+                           get_y(y_index+1), z2);
+        return z0;
+    }
+};
+
+extern mesh_bed_leveling mbl;
+
+#endif  // MESH_BED_LEVELING

Marlin/planner.cpp

 #include "ultralcd.h"
 #include "language.h"
 
+#if defined(MESH_BED_LEVELING)
+  #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
 // Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in 
 // mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration
 // calculation the caller must also provide the physical length of the line in millimeters.
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
 void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder)
 #else
 void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder)
     lcd_update();
   }
 
+#if defined(MESH_BED_LEVELING)
+  z += mbl.get_z(x, y);
+#endif  // MESH_BED_LEVELING
+
 #ifdef ENABLE_AUTO_BED_LEVELING
   apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #endif // ENABLE_AUTO_BED_LEVELING
 }
 #endif // ENABLE_AUTO_BED_LEVELING
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
 void plan_set_position(float x, float y, float z, const float &e)
-{
-  apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
 #else
 void plan_set_position(const float &x, const float &y, const float &z, const float &e)
+#endif  // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 {
-#endif // ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING)
+  apply_rotation_xyz(plan_bed_level_matrix, x, y, z);
+#elif defined(MESH_BED_LEVELING)
+  z += mbl.get_z(x, y);
+#endif  // ENABLE_AUTO_BED_LEVELING
 
   position[X_AXIS] = lround(x*axis_steps_per_unit[X_AXIS]);
   position[Y_AXIS] = lround(y*axis_steps_per_unit[Y_AXIS]);

Marlin/planner.h

 // Add a new linear movement to the buffer. x, y and z is the signed, absolute target position in 
 // millimaters. Feed rate specifies the speed of the motion.
 
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
 void plan_buffer_line(float x, float y, float z, const float &e, float feed_rate, const uint8_t &extruder);
-
 // Get the position applying the bed level matrix if enabled
+#if defined(ENABLE_AUTO_BED_LEVELING)
 vector_3 plan_get_position();
+#endif  // ENABLE_AUTO_BED_LEVELING
 #else
 void plan_buffer_line(const float &x, const float &y, const float &z, const float &e, float feed_rate, const uint8_t &extruder);
-#endif // ENABLE_AUTO_BED_LEVELING
+#endif  // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 
 // Set position. Used for G92 instructions.
-#ifdef ENABLE_AUTO_BED_LEVELING
+#if defined(ENABLE_AUTO_BED_LEVELING) || defined(MESH_BED_LEVELING)
 void plan_set_position(float x, float y, float z, const float &e);
 #else
 void plan_set_position(const float &x, const float &y, const float &z, const float &e);
-#endif // ENABLE_AUTO_BED_LEVELING
+#endif // ENABLE_AUTO_BED_LEVELING || MESH_BED_LEVELING
 
 void plan_set_e_position(const float &e);