delta: Do reverse direction checking in C code

Calculate where a tower must reverse direction during a move in the C
code instead of the delta.py kinematic code.  This simplifies the
python code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

2 files changed, 57 insertions, 67 deletions

diff --git a/klippy/delta.py b/klippy/delta.py
index 8859e17b..ac8d025f 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -1,6 +1,6 @@
 # Code for handling the kinematics of linear delta robots
 #
-# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import math, logging
@@ -165,14 +165,14 @@ class DeltaKinematics:
         if self.need_motor_enable:
             self._check_motor_enable(move_time)
         axes_d = move.axes_d
-        move_d = movexy_d = move.move_d
+        move_d = move.move_d
         movexy_r = 1.
         movez_r = 0.
-        inv_movexy_d = 1. / movexy_d
+        inv_movexy_d = 1. / move_d
         if not axes_d[0] and not axes_d[1]:
             # Z only move
             movez_r = axes_d[2] * inv_movexy_d
-            movexy_d = movexy_r = inv_movexy_d = 0.
+            movexy_r = inv_movexy_d = 0.
         elif axes_d[2]:
             # XY+Z move
             movexy_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
@@ -184,10 +184,9 @@ class DeltaKinematics:
 
         accel = move.accel
         cruise_v = move.cruise_v
-        accel_t = move.accel_t
-        cruise_end_t = accel_t + move.cruise_t
         accel_d = move.accel_r * move_d
-        cruise_end_d = accel_d + move.cruise_r * move_d
+        cruise_d = move.cruise_r * move_d
+        decel_d = move.decel_r * move_d
 
         for i in StepList:
             # Calculate a virtual tower along the line of movement at
@@ -197,70 +196,29 @@ class DeltaKinematics:
             vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
             tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2
             vt_arm_d = math.sqrt(self.arm_length2 - tangentxy_d2)
-
-            # Calculate accel/cruise/decel portions of move
-            reversexy_d = vt_startxy_d + vt_arm_d*movez_r
-            accel_up_d = cruise_up_d = decel_up_d = 0.
-            accel_down_d = cruise_down_d = decel_down_d = 0.
-            if reversexy_d <= 0.:
-                accel_down_d = accel_d
-                cruise_down_d = cruise_end_d
-                decel_down_d = move_d
-            elif reversexy_d >= movexy_d:
-                accel_up_d = accel_d
-                cruise_up_d = cruise_end_d
-                decel_up_d = move_d
-            elif reversexy_d < accel_d * movexy_r:
-                accel_up_d = reversexy_d * move_d * inv_movexy_d
-                accel_down_d = accel_d
-                cruise_down_d = cruise_end_d
-                decel_down_d = move_d
-            elif reversexy_d < cruise_end_d * movexy_r:
-                accel_up_d = accel_d
-                cruise_up_d = reversexy_d * move_d * inv_movexy_d
-                cruise_down_d = cruise_end_d
-                decel_down_d = move_d
-            else:
-                accel_up_d = accel_d
-                cruise_up_d = cruise_end_d
-                decel_up_d = reversexy_d * move_d * inv_movexy_d
-                decel_down_d = move_d
+            vt_startz = origz
 
             # Generate steps
             mcu_stepper = self.steppers[i].mcu_stepper
             mcu_time = mcu_stepper.print_to_mcu_time(move_time)
-            if accel_up_d > 0.:
-                mcu_stepper.step_delta(
-                    mcu_time, accel_up_d, move.start_v, accel,
-                    origz, vt_startxy_d, vt_arm_d, movez_r)
-            if cruise_up_d > 0.:
-                mcu_stepper.step_delta(
-                    mcu_time + accel_t, cruise_up_d - accel_d, cruise_v, 0.,
-                    origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
-                    vt_arm_d, movez_r)
-            if decel_up_d > 0.:
-                mcu_stepper.step_delta(
-                    mcu_time + cruise_end_t, decel_up_d - cruise_end_d,
-                    cruise_v, -accel,
-                    origz + cruise_end_d*movez_r,
-                    vt_startxy_d - cruise_end_d*movexy_r,
-                    vt_arm_d, movez_r)
-            if accel_down_d > 0.:
+            if accel_d:
                 mcu_stepper.step_delta(
-                    mcu_time, -accel_down_d, move.start_v, accel,
-                    origz, vt_startxy_d, vt_arm_d, movez_r)
-            if cruise_down_d > 0.:
+                    mcu_time, accel_d, move.start_v, accel,
+                    vt_startz, vt_startxy_d, vt_arm_d, movez_r)
+                vt_startz += accel_d * movez_r
+                vt_startxy_d -= accel_d * movexy_r
+                mcu_time += move.accel_t
+            if cruise_d:
                 mcu_stepper.step_delta(
-                    mcu_time + accel_t, accel_d - cruise_down_d, cruise_v, 0.,
-                    origz + accel_d*movez_r, vt_startxy_d - accel_d*movexy_r,
-                    vt_arm_d, movez_r)
-            if decel_down_d > 0.:
+                    mcu_time, cruise_d, cruise_v, 0.,
+                    vt_startz, vt_startxy_d, vt_arm_d, movez_r)
+                vt_startz += cruise_d * movez_r
+                vt_startxy_d -= cruise_d * movexy_r
+                mcu_time += move.cruise_t
+            if decel_d:
                 mcu_stepper.step_delta(
-                    mcu_time + cruise_end_t, cruise_end_d - decel_down_d,
-                    cruise_v, -accel,
-                    origz + cruise_end_d*movez_r,
-                    vt_startxy_d - cruise_end_d*movexy_r,
-                    vt_arm_d, movez_r)
+                    mcu_time, decel_d, cruise_v, -accel,
+                    vt_startz, vt_startxy_d, vt_arm_d, movez_r)
 
 
 ######################################################################
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c
index d9eeefc3..761c16c2 100644
--- a/klippy/stepcompress.c
+++ b/klippy/stepcompress.c
@@ -1,6 +1,6 @@
 // Stepper pulse schedule compression
 //
-// Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2016,2017  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 //
@@ -531,8 +531,8 @@ stepcompress_push_const(
 }
 
 // Schedule steps using delta kinematics
-int32_t
-stepcompress_push_delta(
+static int32_t
+_stepcompress_push_delta(
     struct stepcompress *sc, double clock_offset, double move_sd
     , double start_sv, double accel
     , double height, double startxy_sd, double arm_sd, double movez_r)
@@ -627,6 +627,38 @@ stepcompress_push_delta(
     return res;
 }
 
+int32_t
+stepcompress_push_delta(
+    struct stepcompress *sc, double clock_offset, double move_sd
+    , double start_sv, double accel
+    , double height, double startxy_sd, double arm_sd, double movez_r)
+{
+    double reversexy_sd = startxy_sd + arm_sd*movez_r;
+    if (reversexy_sd <= 0.)
+        // All steps are in down direction
+        return _stepcompress_push_delta(
+            sc, clock_offset, -move_sd, start_sv, accel
+            , height, startxy_sd, arm_sd, movez_r);
+    double movexy_r = movez_r ? sqrt(1. - movez_r*movez_r) : 1.;
+    if (reversexy_sd >= move_sd * movexy_r)
+        // All steps are in up direction
+        return _stepcompress_push_delta(
+            sc, clock_offset, move_sd, start_sv, accel
+            , height, startxy_sd, arm_sd, movez_r);
+    // Steps in both up and down direction
+    int res1 = _stepcompress_push_delta(
+        sc, clock_offset, reversexy_sd / movexy_r, start_sv, accel
+        , height, startxy_sd, arm_sd, movez_r);
+    if (res1 == ERROR_RET)
+        return res1;
+    int res2 = _stepcompress_push_delta(
+        sc, clock_offset, -move_sd, start_sv, accel
+        , height + res1, startxy_sd, arm_sd, movez_r);
+    if (res2 == ERROR_RET)
+        return res2;
+    return res1 + res2;
+}
+
 
 /****************************************************************
  * Step compress synchronization