From 0e51e53813c3f2ccd64e09c2cbc16c2740b3974c Mon Sep 17 00:00:00 2001
From: Edward Patel <edward.patel@memention.com>
Date: Sun, 15 Mar 2015 10:43:26 +0100
Subject: [PATCH] WIP. Adding bed leveling code.

---
 Marlin/Configuration.h       | 16 ++++++++
 Marlin/Marlin_main.cpp       | 73 ++++++++++++++++++++++++++++++++++--
 Marlin/mesh_bed_leveling.cpp |  7 ++++
 Marlin/mesh_bed_leveling.h   | 69 ++++++++++++++++++++++++++++++++++
 Marlin/planner.cpp           | 21 ++++++++---
 Marlin/planner.h             | 11 +++---
 6 files changed, 184 insertions(+), 13 deletions(-)
 create mode 100644 Marlin/mesh_bed_leveling.cpp
 create mode 100644 Marlin/mesh_bed_leveling.h

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index a2c43bd3d1..63be50d388 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -372,6 +372,22 @@ const bool Z_MAX_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //const bool FIL_RUNOUT_INVERTING = true;  // Should be uncommented and true or false should assigned
 //#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 ===========================
 //===========================================================================
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index e7298c43cb..b07a2db1ec 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -41,6 +41,10 @@
 
 #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"
@@ -4987,6 +4991,65 @@ void calculate_delta(float cartesian[3])
 }
 #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);
@@ -5102,10 +5165,14 @@ for (int s = 1; s <= steps; s++) {
 #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)
 
diff --git a/Marlin/mesh_bed_leveling.cpp b/Marlin/mesh_bed_leveling.cpp
new file mode 100644
index 0000000000..dc82d376ed
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.cpp
@@ -0,0 +1,7 @@
+#include "mesh_bed_leveling.h"
+
+#if defined(MESH_BED_LEVELING)
+
+mesh_bed_leveling mbl;
+
+#endif  // MESH_BED_LEVELING
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
new file mode 100644
index 0000000000..bd48f564e1
--- /dev/null
+++ b/Marlin/mesh_bed_leveling.h
@@ -0,0 +1,69 @@
+#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
diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp
index 9bcad5661d..1c7a3a9b38 100644
--- a/Marlin/planner.cpp
+++ b/Marlin/planner.cpp
@@ -58,6 +58,10 @@
 #include "ultralcd.h"
 #include "language.h"
 
+#if defined(MESH_BED_LEVELING)
+  #include "mesh_bed_leveling.h"
+#endif  // MESH_BED_LEVELING
+
 //===========================================================================
 //============================= public variables ============================
 //===========================================================================
@@ -530,7 +534,7 @@ float junction_deviation = 0.1;
 // 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)
@@ -548,6 +552,10 @@ void plan_buffer_line(const float &x, const float &y, const float &z, const floa
     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
@@ -1078,14 +1086,17 @@ vector_3 plan_get_position() {
 }
 #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]);
diff --git a/Marlin/planner.h b/Marlin/planner.h
index a64a0f0d3c..22443c05fb 100644
--- a/Marlin/planner.h
+++ b/Marlin/planner.h
@@ -82,21 +82,22 @@ void plan_init();
 // 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);