corexy: Fix new_dda_startpoint for corexy

Add a function axes_um_to_steps to convert from um to steps on all axes
respecting current kinematics setting.

Extend code_stepper_axescode_axes_to_stepper_axes to convert all axes,
including E-axis for consistency.

It seems like axes_um_to_steps could be simplified to something like
"apply_kinematics_axes()" which would just do the transformation math
in-place on some axes[] to move from 'Cartesian' to 'target-kinematics'.
Then the original um_to_steps and delta_um code could remain untouched
since 2014.  But I'm not sure how this will work with scara or delta
configurations. I'm fairly certain they only work from absolute positions
anyway.

Fixes #216.

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;
+	axes_um_to_steps(startpoint.axis, startpoint_steps.axis);
-
-  for (i = X; i < AXIS_COUNT; i++)
-    startpoint_steps.axis[i] = um_to_steps(startpoint.axis[i], i);
 }
 
 /*! 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

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);
 }

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 */