polar: add velocity scaling (#7172)

Print near the origin lead in to fast motor movements, therefore the
movement needs to be scaled down. The start of the scaling is done via
a rotation velocity limit from the config.

Signed-off-by: Nils Hensch <nils.hensch@gmx.de>

config/example-polar.cfg

@@ -72,3 +72,4 @@ max_velocity: 300
 max_accel: 3000
 max_z_velocity: 25
 max_z_accel: 30
+max_angular_velocity: 5

docs/Config_Reference.md

@@ -569,6 +569,11 @@ max_z_accel:
 #   This sets the maximum acceleration (in mm/s^2) of movement along
 #   the z axis. It limits the acceleration of the z stepper motor. The
 #   default is to use max_accel for max_z_accel.
+# max_angular_velocity: 0
+#   This limits the maximum angular velocity (in rad/s) of a move.
+#   Lower values will result in longer print times, but prevents too
+#   fast motions near the center. A value of 0 deactivates the
+#   scaling. The default is to not apply maximum angular velocity limits.
 
 # The stepper_bed section is used to describe the stepper controlling
 # the bed.

klippy/kinematics/polar.py

@@ -6,6 +6,26 @@
 import logging, math
 import stepper
 
+
+def distance_to_center(p1, p2):
+    ab_x = p2[0]-p1[0]
+    ab_y = p2[1]-p1[1]
+    ap_x = -p1[0]
+    ap_y = -p1[1]
+
+    ab_ap_dot_product = ab_x * ap_x + ab_y * ap_y
+    ab_length = math.sqrt(ab_x ** 2 + ab_y ** 2)
+
+    # Check if the projected point lies on the bounded line segment
+    if ab_ap_dot_product <= 0:
+        dist = math.sqrt(ap_x ** 2 + ap_y ** 2)
+    elif ab_ap_dot_product >= ab_length ** 2:
+        dist = math.sqrt(p2[0] ** 2 + p2[1] ** 2)
+    else:
+        dist = abs(ab_x * ap_y - ab_y * ap_x) / ab_length
+    return dist
+
+
 class PolarKinematics:
     def __init__(self, toolhead, config):
         # Setup axis steppers
@@ -22,11 +42,14 @@ class PolarKinematics:
         for s in self.get_steppers():
             s.set_trapq(toolhead.get_trapq())
         # Setup boundary checks
-        max_velocity, max_accel = toolhead.get_max_velocity()
+        self.max_velocity, self.max_accel = toolhead.get_max_velocity()
         self.max_z_velocity = config.getfloat(
-            'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
+            'max_z_velocity', self.max_velocity, above=0.,
+            maxval=self.max_velocity)
         self.max_z_accel = config.getfloat(
-            'max_z_accel', max_accel, above=0., maxval=max_accel)
+            'max_z_accel', self.max_accel, above=0., maxval=self.max_accel)
+        self.v_rad_max = config.getfloat(
+            'max_angular_velocity', above=0., default=0)
         self.limit_z = (1.0, -1.0)
         self.limit_xy2 = -1.
         max_xy = self.rails[0].get_range()[1]
@@ -101,6 +124,20 @@ class PolarKinematics:
             z_ratio = move.move_d / abs(move.axes_d[2])
             move.limit_speed(self.max_z_velocity * z_ratio,
                              self.max_z_accel * z_ratio)
+        # Slow down near center
+        if move.axes_d[0] or move.axes_d[1]:
+            if self.v_rad_max == 0:
+                return
+            min_dist = distance_to_center(move.start_pos[0:2],
+                                              move.end_pos[0:2])
+            if min_dist == 0:
+                return
+            v_angular = math.sqrt(move.max_cruise_v2) / min_dist
+            if self.v_rad_max < v_angular:
+                scale_radius = self.v_rad_max/v_angular
+                move.limit_speed(self.max_velocity * scale_radius,
+                                 self.max_accel * scale_radius)
+
     def get_status(self, eventtime):
         xy_home = "xy" if self.limit_xy2 >= 0. else ""
         z_home = "z" if self.limit_z[0] <= self.limit_z[1] else ""

test/klippy/polar.test

@@ -72,3 +72,45 @@ g1 X-30 Y-25
 g1 X-30 Y25
 g1 X30 Y25
 g1 X30 Y-25
+
+; Moves near center - straight lines on axis
+g1 X30 Y0
+g1 X1 Y0
+g1 X30 Y0
+
+g1 X0 Y30
+g1 X0 Y1
+g1 X0 Y30
+
+g1 X-30 Y0
+g1 X-1 Y0
+g1 X-30 Y0
+
+g1 X0 Y-30
+g1 X0 Y-1
+g1 X0 Y-30
+
+; Moves near center
+g1 X30 Y1
+g1 X-30 Y1
+g1 X30 Y1
+
+g1 X30 Y-1
+g1 X-30 Y-1
+g1 X30 Y-1
+
+g1 X1 Y30
+g1 X1 Y-30
+g1 X1 Y30
+
+g1 X-1 Y30
+g1 X-1 Y-30
+g1 X-1 Y30
+
+g1 X30 Y5
+g1 X-30 Y-1
+g1 X30 Y5
+
+g1 X5 Y30
+g1 X-1 Y-30
+g1 X5 Y30