toolhead: Changed the API to limit junction_v2

Signed-off-by: Dmitry Butyugin <[email protected]>

klippy/extras/resonance_tester.py

@@ -150,25 +150,26 @@ def run_test(self, test_seq, axis, gcmd):
             toolhead.cmd_M204(self.gcode.create_gcode_command(
                 "M204", "M204", {"S": abs(accel)}))
             v = last_v + accel * t_seg
-            if abs(v) < 0.000001:
-                v = 0.
-            last_v2 = last_v * last_v
+            abs_v = abs(v)
+            if abs_v < 0.000001:
+                v = abs_v = 0.
+            abs_last_v = abs(last_v)
             v2 = v * v
+            last_v2 = last_v * last_v
             half_inv_accel = .5 / accel
             d = (v2 - last_v2) * half_inv_accel
             dX, dY = axis.get_point(d)
             nX = X + dX
             nY = Y + dY
+            toolhead.limit_next_junction_speed(abs_last_v)
             if v * last_v < 0:
                 # The move first goes to a complete stop, then changes direction
                 d_decel = -last_v2 * half_inv_accel
                 decel_X, decel_Y = axis.get_point(d_decel)
-                toolhead.move([X + decel_X, Y + decel_Y, Z, E], abs(last_v),
-                              max_junction_v2=last_v2)
-                toolhead.move([nX, nY, Z, E], abs(v))
+                toolhead.move([X + decel_X, Y + decel_Y, Z, E], abs_last_v)
+                toolhead.move([nX, nY, Z, E], abs_v)
             else:
-                toolhead.move([nX, nY, Z, E], max(abs(v), abs(last_v)),
-                              max_junction_v2=last_v2)
+                toolhead.move([nX, nY, Z, E], max(abs_v, abs_last_v))
             if math.floor(freq) > math.floor(last_freq):
                 gcmd.respond_info("Testing frequency %.0f Hz" % (freq,))
                 reactor.pause(reactor.monotonic() + 0.01)

klippy/toolhead.py

@@ -47,7 +47,7 @@ def __init__(self, toolhead, start_pos, end_pos, speed):
         self.delta_v2 = 2.0 * move_d * self.accel
         self.max_smoothed_v2 = 0.
         self.smooth_delta_v2 = 2.0 * move_d * toolhead.max_accel_to_decel
-        self.junction_v2 = 999999999.9
+        self.next_junction_v2 = 999999999.9
     def limit_speed(self, speed, accel):
         speed2 = speed**2
         if speed2 < self.max_cruise_v2:
@@ -56,8 +56,8 @@ def limit_speed(self, speed, accel):
         self.accel = min(self.accel, 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_junction(self, junction_v2):
-        self.junction_v2 = min(self.junction_v2, junction_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])
@@ -67,9 +67,9 @@ def calc_junction(self, prev_move):
             return
         # Allow extruder to calculate its maximum junction
         extruder_v2 = self.toolhead.extruder.calc_junction(prev_move, self)
-        max_start_v2 = min(
-            extruder_v2, self.max_cruise_v2, prev_move.max_cruise_v2,
-            self.junction_v2, prev_move.max_start_v2 + prev_move.delta_v2)
+        max_start_v2 = min(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"
         axes_r = self.axes_r
         prev_axes_r = prev_move.axes_r
@@ -465,16 +465,18 @@ def set_position(self, newpos, homing_axes=()):
         self.commanded_pos[:] = newpos
         self.kin.set_position(newpos, homing_axes)
         self.printer.send_event("toolhead:set_position")
-    def move(self, newpos, speed, max_junction_v2=None):
+    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:
             return
         if move.is_kinematic_move:
             self.kin.check_move(move)
         if move.axes_d[3]:
             self.extruder.check_move(move)
-        if max_junction_v2 is not None:
-            move.limit_junction(max_junction_v2)
         self.commanded_pos[:] = move.end_pos
         self.lookahead.add_move(move)
         if self.print_time > self.need_check_pause: