From 9f30cc84cebc11a51aee54228b9d3c46a259dde7 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Thu, 21 Jul 2016 14:35:48 -0700
Subject: [PATCH] Use set_current_from_steppers for other kinematics

---
 Marlin/Marlin.h        |  1 -
 Marlin/Marlin_main.cpp | 36 ++++++++++++++++++++++++------------
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 252aa9681..0c36168df 100644
--- a/Marlin/Marlin.h
+++ b/Marlin/Marlin.h
@@ -318,7 +318,6 @@ float code_value_temp_diff();
   void calculate_delta(float cartesian[3]);
   void recalc_delta_settings(float radius, float diagonal_rod);
   float delta_safe_distance_from_top();
-  void set_current_from_steppers();
   void set_cartesian_from_steppers();
   void forwardKinematics(float point[3]);
   void forwardKinematics(float z1, float z2, float z3);
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 9c37bfcb8..ea52c0ad3 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -462,7 +462,7 @@ static uint8_t target_extruder;
   #define TOWER_3 Z_AXIS
 
   float delta[3] = { 0 };
-  float cartesian[3] = { 0 };
+  float cartesian_position[3] = { 0 };
   #define SIN_60 0.8660254037844386
   #define COS_60 0.5
   float endstop_adj[3] = { 0 };
@@ -564,6 +564,7 @@ void stop();
 void get_available_commands();
 void process_next_command();
 void prepare_move_to_destination();
+void set_current_from_steppers();
 
 #if ENABLED(ARC_SUPPORT)
   void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
@@ -7801,7 +7802,7 @@ void clamp_to_software_endstops(float target[3]) {
     // based on a Java function from
     // "Delta Robot Kinematics by Steve Graves" V3
 
-    // Result is in cartesian[].
+    // Result is in cartesian_position[].
 
     //Create a vector in old coordinates along x axis of new coordinate
     float p12[3] = { delta_tower2_x - delta_tower1_x, delta_tower2_y - delta_tower1_y, z2 - z1 };
@@ -7845,9 +7846,9 @@ void clamp_to_software_endstops(float target[3]) {
     //Now we can start from the origin in the old coords and
     //add vectors in the old coords that represent the
     //Xnew, Ynew and Znew to find the point in the old system
-    cartesian[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
-    cartesian[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
-    cartesian[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
+    cartesian_position[X_AXIS] = delta_tower1_x + ex[0]*Xnew + ey[0]*Ynew - ez[0]*Znew;
+    cartesian_position[Y_AXIS] = delta_tower1_y + ex[1]*Xnew + ey[1]*Ynew - ez[1]*Znew;
+    cartesian_position[Z_AXIS] = z1             + ex[2]*Xnew + ey[2]*Ynew - ez[2]*Znew;
   };
 
   void forwardKinematics(float point[3]) {
@@ -7860,13 +7861,6 @@ void clamp_to_software_endstops(float target[3]) {
                       stepper.get_axis_position_mm(Z_AXIS));
   }
 
-  void set_current_from_steppers() {
-    set_cartesian_from_steppers();
-    current_position[X_AXIS] = cartesian[X_AXIS];
-    current_position[Y_AXIS] = cartesian[Y_AXIS];
-    current_position[Z_AXIS] = cartesian[Z_AXIS];
-  }
-
   #if ENABLED(AUTO_BED_LEVELING_FEATURE)
 
     // Adjust print surface height by linear interpolation over the bed_level array.
@@ -7911,6 +7905,24 @@ void clamp_to_software_endstops(float target[3]) {
 
 #endif // DELTA
 
+void set_current_from_steppers() {
+  #if ENABLED(DELTA)
+    set_cartesian_from_steppers();
+    current_position[X_AXIS] = cartesian_position[X_AXIS];
+    current_position[Y_AXIS] = cartesian_position[Y_AXIS];
+    current_position[Z_AXIS] = cartesian_position[Z_AXIS];
+  #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
+    vector_3 pos = planner.adjusted_position(); // values directly from steppers...
+    current_position[X_AXIS] = pos.x;
+    current_position[Y_AXIS] = pos.y;
+    current_position[Z_AXIS] = pos.z;
+  #else
+    current_position[X_AXIS] = stepper.get_axis_position_mm(X_AXIS); // CORE handled transparently
+    current_position[Y_AXIS] = stepper.get_axis_position_mm(Y_AXIS);
+    current_position[Z_AXIS] = stepper.get_axis_position_mm(Z_AXIS);
+  #endif
+}
+
 #if ENABLED(MESH_BED_LEVELING)
 
 // This function is used to split lines on mesh borders so each segment is only part of one mesh area