delta: Cap maximum stepper velocity and acceleration

Some XY moves at the extreme end of the build envelope could cause
excessive axis stepper movement.  Check for any moves that could
possibly result in a stepper movement of more than 3 times the XY
movement and cap the move's acceleration and speed.

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

2 files changed, 47 insertions, 3 deletions

diff --git a/config/example-delta.cfg b/config/example-delta.cfg
index 01ab6482..96ab91b3 100644
--- a/config/example-delta.cfg
+++ b/config/example-delta.cfg
@@ -79,7 +79,17 @@ pin_map: arduino
 kinematics: delta
 #   This option must be "delta" for linear delta printers.
 max_velocity: 300
+#   Maximum velocity (in mm/s) of the toolhead relative to the
+#   print. This limits the velocity of the toolhead relative to the
+#   print - at the extreme end of the print envelope the delta axis
+#   steppers themselves may briefly exceed this speed by up to 3
+#   times. This parameter must be specified.
 max_accel: 3000
+#   Maximum acceleration (in mm/s^2) of the toolhead relative to the
+#   print. This limits the acceleration of the toolhead relative to
+#   the print - at the extreme end of the print envelope the delta
+#   axis steppers may briefly exceed this acceleration by up to 3
+#   times. This parameter must be specified.
 max_z_velocity: 200
 #   For delta printers this limits the maximum velocity (in mm/s) of
 #   moves with z axis movement. This setting can be used to reduce the
diff --git a/klippy/delta.py b/klippy/delta.py
index 078c6418..914783eb 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -8,14 +8,18 @@ import stepper, homing
 
 StepList = (0, 1, 2)
 
+# Slow moves once the ratio of tower to XY movement exceeds SLOW_RATIO
+SLOW_RATIO = 3.
+
 class DeltaKinematics:
     def __init__(self, printer, config):
+        self.config = config
         self.steppers = [stepper.PrinterStepper(
             printer, config.getsection('stepper_' + n), n)
                          for n in ['a', 'b', 'c']]
         self.need_motor_enable = self.need_home = True
-        self.max_z_velocity = config.getfloat('max_z_velocity', 9999999.9)
-        radius = config.getfloat('delta_radius')
+        self.max_velocity = self.max_z_velocity = 0.
+        self.radius = radius = config.getfloat('delta_radius')
         arm_length = config.getfloat('delta_arm_length')
         self.arm_length2 = arm_length**2
         self.max_xy2 = min(radius, arm_length - radius)**2
@@ -23,13 +27,29 @@ class DeltaKinematics:
         tower_height_at_zeros = math.sqrt(self.arm_length2 - radius**2)
         self.max_z = self.steppers[0].position_max
         self.limit_z = self.max_z - (arm_length - tower_height_at_zeros)
+        logging.info(
+            "Delta max build height %.2fmm (radius tapered above %.2fmm)" % (
+                self.max_z, self.limit_z))
         sin = lambda angle: math.sin(math.radians(angle))
         cos = lambda angle: math.cos(math.radians(angle))
         self.towers = [
             (cos(210.)*radius, sin(210.)*radius),
             (cos(330.)*radius, sin(330.)*radius),
             (cos(90.)*radius, sin(90.)*radius)]
+        # Find the point where an XY move could result in excessive
+        # tower movement
+        half_min_step_dist = min([s.step_dist for s in self.steppers]) * .5
+        dist = (SLOW_RATIO * math.sqrt(self.arm_length2 / (SLOW_RATIO**2 + 1.)
+                                       - half_min_step_dist**2)
+                + half_min_step_dist)
+        self.slow_xy2 = (dist - radius)**2
+        logging.info(
+            "Delta max build radius %.2fmm (moves slowed past %.2fmm)" % (
+                math.sqrt(self.max_xy2), math.sqrt(self.slow_xy2)))
     def set_max_jerk(self, max_xy_halt_velocity, max_velocity, max_accel):
+        self.max_velocity = max_velocity
+        max_z_velocity = self.config.getfloat('max_z_velocity', max_velocity)
+        self.max_z_velocity = min(max_velocity, max_z_velocity)
         for stepper in self.steppers:
             stepper.set_max_jerk(max_xy_halt_velocity, max_accel)
     def build_config(self):
@@ -128,7 +148,21 @@ class DeltaKinematics:
         if move.axes_d[2]:
             move.limit_speed(self.max_z_velocity, move.accel)
             limit_xy2 = -1.
-        self.limit_xy2 = limit_xy2
+        # Limit the speed/accel of this move if is is at the extreme
+        # end of the build envelope
+        extreme_xy2 = max(xy2, move.start_pos[0]**2 + move.start_pos[1]**2)
+        if extreme_xy2 > self.slow_xy2:
+            min_step = min([s.step_dist for s in self.steppers])
+            d = math.sqrt(extreme_xy2) + self.radius
+            r = SLOW_RATIO * min_step / (
+                math.sqrt(self.arm_length2 - (d - min_step)**2)
+                - math.sqrt(self.arm_length2 - d**2))
+            max_velocity = self.max_velocity
+            if move.axes_d[2]:
+                max_velocity = self.max_z_velocity
+            move.limit_speed(max_velocity * r, move.accel * r)
+            limit_xy2 = -1.
+        self.limit_xy2 = min(limit_xy2, self.slow_xy2)
     def move(self, move_time, move):
         axes_d = move.axes_d
         move_d = movexy_d = move.move_d