toolhead: Fixed junction deviation calculation for straight segments …

…(#6747) (#471)

* toolhead: Use delta_v2 when calculating centripetal force

As a minor math optimization, it's possible to calculate:
  .5 * self.move_d * self.accel * tan_theta_d2
using:
  self.delta_v2 * .25 * tan_theta_d2
because self.delta_v2 is "2. * self.move_d * self.accel".

Signed-off-by: Dmitry Butyugin <[email protected]>
Signed-off-by: Kevin O'Connor <[email protected]>

* toolhead: Remove arbitrary constants controlling junction deviation

When calculating the junction speed between two moves the code checked
for angles greater than 0.999999 or less than -0.999999 to avoid math
issues (sqrt of a negative number and/or divide by zero).  However,
these arbitrary constants could unnecessarily pessimize junction
speeds when angles are close to 180 or 0 degrees.

Change the code to explicitly check for negative numbers during sqrt
and to explicilty check for zero values prior to division.  This
simplifies the code and avoids unnecessarily reducing some junction
speeds.

Signed-off-by: Dmitry Butyugin <[email protected]>
Signed-off-by: Kevin O'Connor <[email protected]>

---------

Signed-off-by: Dmitry Butyugin <[email protected]>
Signed-off-by: Kevin O'Connor <[email protected]>
Co-authored-by: Kevin O'Connor <[email protected]>

klippy/toolhead.py

@@ -20,6 +20,7 @@ def __init__(self, toolhead, start_pos, end_pos, speed):
         self.start_pos = tuple(start_pos)
         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)
@@ -74,6 +75,12 @@ 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,
+            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
@@ -82,33 +89,29 @@ def calc_junction(self, prev_move):
             + axes_r[1] * prev_axes_r[1]
             + axes_r[2] * prev_axes_r[2]
         )
-        if junction_cos_theta > 0.999999:
-            return
-        junction_cos_theta = max(junction_cos_theta, -0.999999)
-        sin_theta_d2 = math.sqrt(0.5 * (1.0 - junction_cos_theta))
-        R_jd = sin_theta_d2 / (1.0 - sin_theta_d2)
-        # Approximated circle must contact moves no further away than mid-move
-        tan_theta_d2 = sin_theta_d2 / math.sqrt(
-            0.5 * (1.0 + junction_cos_theta)
-        )
-        move_centripetal_v2 = 0.5 * self.move_d * tan_theta_d2 * self.accel
-        prev_move_centripetal_v2 = (
-            0.5 * prev_move.move_d * tan_theta_d2 * prev_move.accel
-        )
+        sin_theta_d2 = math.sqrt(max(0.5 * (1.0 - junction_cos_theta), 0.0))
+        cos_theta_d2 = math.sqrt(max(0.5 * (1.0 + junction_cos_theta), 0.0))
+        one_minus_sin_theta_d2 = 1.0 - sin_theta_d2
+        if one_minus_sin_theta_d2 > 0.0 and cos_theta_d2 > 0.0:
+            R_jd = sin_theta_d2 / one_minus_sin_theta_d2
+            move_jd_v2 = R_jd * self.junction_deviation * self.accel
+            pmove_jd_v2 = R_jd * prev_move.junction_deviation * prev_move.accel
+            # Approximated circle must contact moves no further than mid-move
+            #   centripetal_v2 = .5 * self.move_d * self.accel * tan_theta_d2
+            quarter_tan_theta_d2 = 0.25 * sin_theta_d2 / cos_theta_d2
+            move_centripetal_v2 = self.delta_v2 * quarter_tan_theta_d2
+            pmove_centripetal_v2 = prev_move.delta_v2 * quarter_tan_theta_d2
+            max_start_v2 = min(
+                max_start_v2,
+                move_jd_v2,
+                pmove_jd_v2,
+                move_centripetal_v2,
+                pmove_centripetal_v2,
+            )
         # Apply limits
-        self.max_start_v2 = min(
-            R_jd * self.equilateral_corner_v2,
-            R_jd * prev_move.equilateral_corner_v2,
-            move_centripetal_v2,
-            prev_move_centripetal_v2,
-            extruder_v2,
-            self.max_cruise_v2,
-            prev_move.max_cruise_v2,
-            prev_move.max_start_v2 + prev_move.delta_v2,
-        )
+        self.max_start_v2 = max_start_v2
         self.max_smoothed_v2 = min(
-            self.max_start_v2,
-            prev_move.max_smoothed_v2 + prev_move.smooth_delta_v2,
+            max_start_v2, prev_move.max_smoothed_v2 + prev_move.smooth_delta_v2
         )
 
     def set_junction(self, start_v2, cruise_v2, end_v2):
@@ -277,7 +280,7 @@ def __init__(self, config):
             "square_corner_velocity", 5.0, minval=0.0
         )
         self.equilateral_corner_v2 = 0.0
-        self.max_accel_to_decel = 0.0
+        self.junction_deviation = self.max_accel_to_decel = 0
         self._calc_junction_deviation()
         # Input stall detection
         self.check_stall_time = 0.0