diff --git a/dda.c b/dda.c
index 572078d..bf5db44 100644
--- a/dda.c
+++ b/dda.c
@@ -117,10 +117,7 @@ void dda_init(void) {
 	This is needed for example after homing or a G92. The new location must be in startpoint already.
 */
 void dda_new_startpoint(void) {
-  enum axis_e i;
-
-  for (i = X; i < AXIS_COUNT; i++)
-    startpoint_steps.axis[i] = um_to_steps(startpoint.axis[i], i);
+	axes_um_to_steps(startpoint.axis, startpoint_steps.axis);
 }
 
 /*! CREATE a dda given current_position and a target, save to passed location so we can write directly into the queue
@@ -218,9 +215,6 @@ void dda_create(DDA *dda, TARGET *target) {
   if ( ! target->e_relative) {
     int32_t delta_steps;
 
-    delta_um[E] = (uint32_t)labs(target->axis[E] - startpoint.axis[E]);
-    steps[E] = um_to_steps(target->axis[E], E);
-
     delta_steps = steps[E] - startpoint_steps.axis[E];
     dda->delta[E] = (uint32_t)labs(delta_steps);
     startpoint_steps.axis[E] = steps[E];
@@ -240,7 +234,7 @@ void dda_create(DDA *dda, TARGET *target) {
     // When we get more extruder axes:
     // for (i = E; i < AXIS_COUNT; i++) { ...
     delta_um[E] = (uint32_t)labs(target->axis[E]);
-    dda->delta[E] = (uint32_t)labs(um_to_steps(target->axis[E], E));
+    dda->delta[E] = (uint32_t)labs(steps[E]);
     #ifdef LOOKAHEAD
       dda->delta_um[E] = target->axis[E];
     #endif
diff --git a/dda_kinematics.c b/dda_kinematics.c
index 45244e8..a45221d 100644
--- a/dda_kinematics.c
+++ b/dda_kinematics.c
@@ -13,7 +13,7 @@ carthesian_to_carthesian(TARGET *startpoint, TARGET *target,
                          axes_uint32_t delta_um, axes_int32_t steps) {
   enum axis_e i;
 
-  for (i = X; i < E; i++) {
+  for (i = X; i < AXIS_COUNT; i++) {
     delta_um[i] = (uint32_t)labs(target->axis[i] - startpoint->axis[i]);
     steps[i] = um_to_steps(target->axis[i], i);
   }
@@ -25,12 +25,24 @@ carthesian_to_corexy(TARGET *startpoint, TARGET *target,
 
   delta_um[X] = (uint32_t)labs((target->axis[X] - startpoint->axis[X]) +
                                (target->axis[Y] - startpoint->axis[Y]));
-  steps[X] = um_to_steps(target->axis[X] + target->axis[Y], X);
-
   delta_um[Y] = (uint32_t)labs((target->axis[X] - startpoint->axis[X]) -
                                (target->axis[Y] - startpoint->axis[Y]));
-  steps[Y] = um_to_steps(target->axis[X] - target->axis[Y], Y);
-
   delta_um[Z] = (uint32_t)labs(target->axis[Z] - startpoint->axis[Z]);
-  steps[Z] = um_to_steps(target->axis[Z], Z);
+  delta_um[E] = (uint32_t)labs(target->axis[E] - startpoint->axis[E]);
+  axes_um_to_steps_corexy(target->axis, steps);
+}
+
+void axes_um_to_steps_cartesian(const axes_int32_t um, axes_int32_t steps) {
+  enum axis_e i;
+
+  for (i = X; i < AXIS_COUNT; i++) {
+    steps[i] = um_to_steps(um[i], i);
+  }
+}
+
+void axes_um_to_steps_corexy(const axes_int32_t um, axes_int32_t steps) {
+  steps[X] = um_to_steps(um[X] + um[Y], X);
+  steps[Y] = um_to_steps(um[X] - um[Y], Y);
+  steps[Z] = um_to_steps(um[Z], Z);
+  steps[E] = um_to_steps(um[E], E);
 }
diff --git a/dda_kinematics.h b/dda_kinematics.h
index baf51f5..fef2f4c 100644
--- a/dda_kinematics.h
+++ b/dda_kinematics.h
@@ -34,4 +34,22 @@ inline void code_axes_to_stepper_axes(TARGET *startpoint, TARGET *target,
   #endif
 }
 
+void axes_um_to_steps_cartesian(const axes_int32_t um, axes_int32_t steps);
+void axes_um_to_steps_corexy(const axes_int32_t um, axes_int32_t steps);
+// void axes_um_to_steps_scara(const axes_int32_t um, axes_int32_t steps);
+
+static void axes_um_to_steps(const axes_int32_t, axes_int32_t)
+                                      __attribute__ ((always_inline));
+inline void axes_um_to_steps(const axes_int32_t um, axes_int32_t steps) {
+  #if defined KINEMATICS_STRAIGHT
+    axes_um_to_steps_cartesian(um, steps);
+  #elif defined KINEMATICS_COREXY
+    axes_um_to_steps_corexy(um, steps);
+//  #elif defined KINEMATICS_SCARA
+//    axes_um_to_steps_scara(um, steps);
+  #else
+    #error KINEMATICS not defined or unknown, edit your config.h.
+  #endif
+}
+
 #endif /* _DDA_KINEMATICS_H */