From 34fd59c3704b830985721d2694706e9d903c9b4a Mon Sep 17 00:00:00 2001
From: Gabe Rosenhouse <gabe@missionst.com>
Date: Sat, 15 Feb 2014 18:06:51 -0800
Subject: [PATCH] ABL at any points

---
 Marlin/Configuration.h | 16 ++++++++++++++
 Marlin/Marlin_main.cpp | 49 ++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
index c9f3558d41..839bc3482e 100644
--- a/Marlin/Configuration.h
+++ b/Marlin/Configuration.h
@@ -335,12 +335,28 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
 
 #ifdef ENABLE_AUTO_BED_LEVELING
 
+// Enable auto bed leveling at any 3 points that aren't colinear
+#define AUTO_BED_LEVELING_ANY_POINTS
+
+#ifdef AUTO_BED_LEVELING_ANY_POINTS
+  #define ABL_PROBE_PT_1_X -11
+  #define ABL_PROBE_PT_1_Y -15
+  #define ABL_PROBE_PT_2_X -11
+  #define ABL_PROBE_PT_2_Y 75
+  #define ABL_PROBE_PT_3_X 121
+  #define ABL_PROBE_PT_3_Y -15
+
+
+#else // not AUTO_BED_LEVELING_ANY_POINTS
+
   // these are the positions on the bed to do the probing
   #define LEFT_PROBE_BED_POSITION 15
   #define RIGHT_PROBE_BED_POSITION 170
   #define BACK_PROBE_BED_POSITION 180
   #define FRONT_PROBE_BED_POSITION 20
 
+#endif
+
   // these are the offsets to the probe relative to the extruder tip (Hotend - Probe)
   #define X_PROBE_OFFSET_FROM_EXTRUDER -25
   #define Y_PROBE_OFFSET_FROM_EXTRUDER -29
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index d92d1e2ce8..8a6542b1f2 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -846,7 +846,36 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
 
-#else
+#else // not ACCURATE_BED_LEVELING
+
+  #ifdef AUTO_BED_LEVELING_ANY_POINTS
+static void set_bed_level_equation_any_pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
+
+    plan_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 from_2_to_1 = (pt1 - pt2).get_normal();
+    vector_3 from_2_to_3 = (pt3 - pt2).get_normal();
+    vector_3 planeNormal = vector_3::cross(from_2_to_1, from_2_to_3).get_normal();
+    planeNormal = vector_3(planeNormal.x, planeNormal.y, abs(planeNormal.z));
+
+    plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
+
+    vector_3 corrected_position = plan_get_position();
+    current_position[X_AXIS] = corrected_position.x;
+    current_position[Y_AXIS] = corrected_position.y;
+    current_position[Z_AXIS] = corrected_position.z;
+
+    // but the bed at 0 so we don't go below it.
+    current_position[Z_AXIS] = zprobe_zoffset;
+
+    plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
+
+}
+  #else // not AUTO_BED_LEVELING_ANY_POINTS
 static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
     plan_bed_level_matrix.set_to_identity();
 
@@ -881,6 +910,7 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
 
     plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
+  #endif // AUTO_BED_LEVELING_ANY_POINTS
 #endif // ACCURATE_BED_LEVELING
 
 static void run_z_probe() {
@@ -1514,6 +1544,21 @@ void process_commands()
 #else // ACCURATE_BED_LEVELING not defined
 
 
+  #ifdef AUTO_BED_LEVELING_ANY_POINTS
+            // probe 1
+            float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
+
+            // probe 2
+            float z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+
+            // probe 3
+            float z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
+
+            clean_up_after_endstop_move();
+
+            set_bed_level_equation_any_pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
+  #else // not AUTO_BED_LEVELING_ANY_POINTS
+
             // prob 1
             float z_at_xLeft_yBack = probe_pt(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, Z_RAISE_BEFORE_PROBING);
 
@@ -1526,7 +1571,7 @@ void process_commands()
             clean_up_after_endstop_move();
 
             set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
-
+  #endif
 
 #endif // ACCURATE_BED_LEVELING
             st_synchronize();