diff --git a/config/example-polar.cfg b/config/example-polar.cfg index 90840fa12..c26529d90 100644 --- a/config/example-polar.cfg +++ b/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 diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 5cf0747ca..5a7e76c02 100644 --- a/docs/Config_Reference.md +++ b/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. diff --git a/klippy/kinematics/polar.py b/klippy/kinematics/polar.py index 422c9981f..52be9193f 100644 --- a/klippy/kinematics/polar.py +++ b/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 "" diff --git a/test/klippy/polar.test b/test/klippy/polar.test index 96346db90..8a81d6e5d 100644 --- a/test/klippy/polar.test +++ b/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