From 5cf1b34924b92890ec465373292ae4785d436ec1 Mon Sep 17 00:00:00 2001
From: Nico Tonnhofer <wurstnase.reprap@gmail.com>
Date: Mon, 17 Apr 2017 09:07:08 +0200
Subject: [PATCH] dda.c: correct current position for CoreXY

---
 dda.c            | 11 +++++++++--
 dda_kinematics.c | 14 ++++++++++++++
 dda_kinematics.h | 18 ++++++++++++++++++
 3 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/dda.c b/dda.c
index 48dbd8e..73e49e9 100644
--- a/dda.c
+++ b/dda.c
@@ -952,14 +952,21 @@ void update_current_position() {
 
   if (dda != NULL) {
     uint32_t axis_um;
+    axes_int32_t delta_um, um;
 
     for (i = X; i < AXIS_COUNT; i++) {
       axis_um = muldiv(move_state.steps[i], 
                        1000000, 
                        pgm_read_dword(&steps_per_m_P[i]));
 
-      current_position.axis[i] =
-        dda->endpoint.axis[i] - (int32_t)get_direction(dda, i) * axis_um;
+      delta_um[i] = (int32_t)get_direction(dda, i) * axis_um;
+    }
+
+    delta_to_axes(delta_um, um);
+    um[E] = delta_um[E];
+
+    for (i = X; i < AXIS_COUNT; i++) {
+      current_position.axis[i] = dda->endpoint.axis[i] - um[i];
     }
 
     if (dda->endpoint.e_relative) {
diff --git a/dda_kinematics.c b/dda_kinematics.c
index 6b0ee9c..ddcb180 100644
--- a/dda_kinematics.c
+++ b/dda_kinematics.c
@@ -55,3 +55,17 @@ void axes_um_to_steps_corexy(const axes_int32_t um, axes_int32_t steps) {
   steps[Y] = um_to_steps(um[X] - um[Y], Y);
   steps[Z] = um_to_steps(um[Z], Z);
 }
+
+void delta_to_axes_cartesian(const axes_int32_t delta_um, axes_int32_t um) {
+  enum axis_e i;
+
+  for (i = X; i < E; i++) {
+    um[i] = delta_um[i];
+  }
+}
+
+void delta_to_axes_corexy(const axes_int32_t delta_um, axes_int32_t um) {
+  um[X] = (delta_um[X] + delta_um[Y]) / 2;
+  um[Y] = (delta_um[X] - delta_um[Y]) / 2;
+  um[Z] = delta_um[Z];
+}
diff --git a/dda_kinematics.h b/dda_kinematics.h
index 155ada6..6203084 100644
--- a/dda_kinematics.h
+++ b/dda_kinematics.h
@@ -52,4 +52,22 @@ inline void axes_um_to_steps(const axes_int32_t um, axes_int32_t steps) {
   #endif
 }
 
+void delta_to_axes_cartesian(const axes_int32_t delta_um, axes_int32_t um);
+void delta_to_axes_corexy(const axes_int32_t delta_um, axes_int32_t um);
+// void delta_to_axes_scara(const axes_int32_t delta_um, axes_int32_t um);
+
+static void delta_to_axes(const axes_int32_t, axes_int32_t)
+                                        __attribute__ ((always_inline));
+inline void delta_to_axes(const axes_int32_t delta_um, axes_int32_t um) {
+  #if defined KINEMATICS_STRAIGHT
+    delta_to_axes_cartesian(delta_um, um);
+  #elif defined KINEMATICS_COREXY
+    delta_to_axes_corexy(delta_um, um);
+//  #elif defined KINEMATICS_SCARA
+//    delta_to_axes_scara(delta_um, um);
+  #else
+    #error KINEMATICS not defined or unknown, edit your config.h.
+  #endif
+}
+
 #endif /* _DDA_KINEMATICS_H */