diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 1cf5cba8c..8c8977edc 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -21,7 +21,6 @@ def __init__(self, toolhead, start_pos, end_pos, speed): self.end_pos = tuple(end_pos) self.accel = toolhead.max_accel self.junction_deviation = toolhead.junction_deviation - self.equilateral_corner_v2 = toolhead.equilateral_corner_v2 self.timing_callbacks = [] velocity = min(speed, toolhead.max_velocity) self.is_kinematic_move = True @@ -55,6 +54,7 @@ def __init__(self, toolhead, start_pos, end_pos, speed): self.delta_v2 = 2.0 * move_d * self.accel self.max_smoothed_v2 = 0.0 self.smooth_delta_v2 = 2.0 * move_d * toolhead.max_accel_to_decel + self.next_junction_v2 = 999999999.9 def limit_speed(self, speed, accel): speed2 = speed**2 @@ -65,6 +65,9 @@ def limit_speed(self, speed, accel): self.delta_v2 = 2.0 * self.move_d * self.accel self.smooth_delta_v2 = min(self.smooth_delta_v2, self.delta_v2) + def limit_next_junction_speed(self, speed): + self.next_junction_v2 = min(self.next_junction_v2, speed**2) + def move_error(self, msg="Move out of range"): ep = self.end_pos m = "%s: %.3f %.3f %.3f [%.3f]" % (msg, ep[0], ep[1], ep[2], ep[3]) @@ -79,6 +82,7 @@ def calc_junction(self, prev_move): extruder_v2, self.max_cruise_v2, prev_move.max_cruise_v2, + prev_move.next_junction_v2, prev_move.max_start_v2 + prev_move.delta_v2, ) # Find max velocity using "approximated centripetal velocity" @@ -279,7 +283,6 @@ def __init__(self, config): self.square_corner_velocity = config.getfloat( "square_corner_velocity", 5.0, minval=0.0 ) - self.equilateral_corner_v2 = 0.0 self.junction_deviation = self.max_accel_to_decel = 0 self._calc_junction_deviation() # Input stall detection @@ -577,6 +580,11 @@ def set_position(self, newpos, homing_axes=()): self.kin.set_position(newpos, homing_axes) self.printer.send_event("toolhead:set_position") + def limit_next_junction_speed(self, speed): + last_move = self.lookahead.get_last() + if last_move is not None: + last_move.limit_next_junction_speed(speed) + def move(self, newpos, speed): move = Move(self, self.commanded_pos, newpos, speed) if not move.move_d: @@ -751,7 +759,7 @@ def get_max_velocity(self): def _calc_junction_deviation(self): scv2 = self.square_corner_velocity**2 - self.equilateral_corner_v2 = scv2 * (math.sqrt(2.0) - 1.0) + self.junction_deviation = scv2 * (math.sqrt(2.0) - 1.0) / self.max_accel self.max_accel_to_decel = self.max_accel * (1.0 - self.min_cruise_ratio) def cmd_G4(self, gcmd):