From dcbe832d9f57563c34b5ebb8c9c256b2d497e4ab Mon Sep 17 00:00:00 2001 From: Ghostkeeper Date: Tue, 4 Sep 2018 16:38:50 +0200 Subject: [PATCH] Implement the rest of the initial time estimation This doesn't complete this task yet because we are still not outputting a correct time estimation for these commands. We're not outputting anything at all actually. We now need to implement the second pass (but only for G1 and G0). Contributes to issue CURA-5561. --- 50_inst_per_sec.py | 34 +++++++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/50_inst_per_sec.py b/50_inst_per_sec.py index cd298899d7..802cb0edd6 100644 --- a/50_inst_per_sec.py +++ b/50_inst_per_sec.py @@ -14,6 +14,7 @@ from typing import Dict, List, Optional, Tuple # ==================================== DEFAULT_BUFFER_FILLING_RATE_IN_C_PER_MS = 50.0 / 1000.0 # The buffer filling rate in #commands/ms DEFAULT_BUFFER_SIZE = 15 # The buffer size in #commands +MINIMUM_PLANNER_SPEED = 0.05 #Setting values for Ultimaker S5. MACHINE_MAX_FEEDRATE_X = 300 @@ -86,6 +87,12 @@ def calc_intersection_distance(initial_feedrate: float, final_feedrate: float, a return 0 return (2 * acceleration * distance - initial_feedrate * initial_feedrate + final_feedrate * final_feedrate) / (4 * acceleration) +## Calculates the maximum speed that is allowed at this point when you must be +# able to reach target_velocity using the acceleration within the allotted +# distance. +def calc_max_allowable_speed(acceleration: float, target_velocity: float, distance: float) -> float: + return math.sqrt(target_velocity * target_velocity - 2 * acceleration * distance) + class Command: def __init__(self, cmd_str: str) -> None: self._cmd_str = cmd_str # type: str @@ -251,7 +258,28 @@ class Command: vmax_junction = min(vmax_junction, self._nominal_feedrate) safe_speed = vmax_junction - #TODO: Compute junction maximum speed factor and apply this to entry speed, set flags and calculate trapezoid. + if buf.previous_nominal_feedrate > 0.0001: + xy_jerk = math.sqrt((current_feedrate[0] - buf.previous_feedrate[0]) ** 2 + (current_feedrate[1] - buf.previous_feedrate[1]) ** 2) + vmax_junction = self._nominal_feedrate + if xy_jerk > MACHINE_MAX_JERK_XY: + vmax_junction_factor = MACHINE_MAX_JERK_XY / xy_jerk + if abs(current_feedrate[2] - buf.previous_feedrate[2]) > MACHINE_MAX_JERK_Z: + vmax_junction_factor = min(vmax_junction_factor, (MACHINE_MAX_JERK_Z / abs(current_feedrate[2] - buf.previous_feedrate[2]))) + if abs(current_feedrate[3] - buf.previous_feedrate[3]) > MACHINE_MAX_JERK_E: + vmax_junction_factor = min(vmax_junction_factor, (MACHINE_MAX_JERK_E / abs(current_feedrate[3] - buf.previous_feedrate[3]))) + vmax_junction = min(buf.previous_nominal_feedrate, vmax_junction * vmax_junction_factor) #Limit speed to max previous speed. + + self._max_entry_speed = vmax_junction + v_allowable = calc_max_allowable_speed(-self._acceleration, MINIMUM_PLANNER_SPEED, self._distance) + self._entry_speed = min(vmax_junction, v_allowable) + self._nominal_length = self._nominal_feedrate <= v_allowable + self._recalculate = True + + buf.previous_feedrate = current_feedrate + buf.previous_nominal_feedrate = self._nominal_feedrate + buf.current_position = new_position + + self.calculate_trapezoid(self._entry_speed / self._nominal_feedrate, safe_speed / self._nominal_feedrate) travel_time_in_ms = distance / (self._nominal_feedrate / 60.0) * 1000.0 @@ -389,6 +417,10 @@ class CommandBuffer: self._detection_time_frame = lower_bound_buffer_depletion_time self._code_count_limit = self._buffer_size + + self.previous_feedrate = [0, 0, 0, 0] + self.previous_nominal_feedrate = 0 + print("Time Frame: %s" % self._detection_time_frame) print("Code Limit: %s" % self._code_count_limit)