diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 4149d53b12f6..f90f5dd0289c 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -64,32 +64,32 @@ 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 junction_cos_theta = -(axes_r[0] * prev_axes_r[0] + 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. - 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 = .5 * self.move_d * tan_theta_d2 * self.accel - prev_move_centripetal_v2 = (.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.)) + cos_theta_d2 = math.sqrt(max(0.5*(1.0+junction_cos_theta), 0.)) + one_minus_sin_theta_d2 = 1. - sin_theta_d2 + if one_minus_sin_theta_d2 > 0. and cos_theta_d2 > 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 + quarter_tan_theta_d2 = .25 * sin_theta_d2 / cos_theta_d2 + move_centripetal_v2 = quarter_tan_theta_d2 * self.delta_v2 + pmove_centripetal_v2 = quarter_tan_theta_d2 * prev_move.delta_v2 + 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.junction_deviation * self.accel, - R_jd * prev_move.junction_deviation * prev_move.accel, - 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): # Determine accel, cruise, and decel portions of the move distance half_inv_accel = .5 / self.accel