diff --git a/klippy/extras/resonance_tester.py b/klippy/extras/resonance_tester.py index fe8717d58545..f5c64fedf8f2 100644 --- a/klippy/extras/resonance_tester.py +++ b/klippy/extras/resonance_tester.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import logging, math, os, time -from . import shaper_calibrate +from . import bulk_sensor, shaper_calibrate class TestAxis: def __init__(self, axis=None, vib_dir=None): @@ -45,10 +45,8 @@ def _parse_axis(gcmd, raw_axis): "Unable to parse axis direction '%s'" % (raw_axis,)) return TestAxis(vib_dir=(dir_x, dir_y)) -class VibrationPulseTest: +class VibrationPulseTestGenerator: def __init__(self, config): - self.printer = config.get_printer() - self.gcode = self.printer.lookup_object('gcode') self.min_freq = config.getfloat('min_freq', 5., minval=1.) # Defaults are such that max_freq * accel_per_hz == 10000 (max_accel) self.max_freq = config.getfloat('max_freq', 10000. / 75., @@ -56,29 +54,52 @@ def __init__(self, config): self.accel_per_hz = config.getfloat('accel_per_hz', 75., above=0.) self.hz_per_sec = config.getfloat('hz_per_sec', 1., minval=0.1, maxval=2.) - - self.probe_points = config.getlists('probe_points', seps=(',', '\n'), - parser=float, count=3) - def get_start_test_points(self): - return self.probe_points def prepare_test(self, gcmd): self.freq_start = gcmd.get_float("FREQ_START", self.min_freq, minval=1.) self.freq_end = gcmd.get_float("FREQ_END", self.max_freq, minval=self.freq_start, maxval=300.) self.hz_per_sec = gcmd.get_float("HZ_PER_SEC", self.hz_per_sec, above=0., maxval=2.) - def run_test(self, axis, gcmd): + def gen_test(self): + freq = self.freq_start + res = [] + sign = 1. + time = 0. + while freq <= self.freq_end + 0.000001: + t_seg = .25 / freq + accel = self.accel_per_hz * freq + time += t_seg + res.append((time, sign * accel)) + time += t_seg + res.append((time, -sign * accel)) + freq += 2. * t_seg * self.hz_per_sec + sign = -sign + return res + def get_params(self): + return { + 'freq_start': self.freq_start, + 'freq_end': self.freq_end, + 'accel_per_hz': self.accel_per_hz, + 'hz_per_sec': self.hz_per_sec, + } + def get_max_freq(self): + return self.freq_end + +class ResonanceTestExecutor: + def __init__(self, config): + self.printer = config.get_printer() + self.gcode = self.printer.lookup_object('gcode') + def run_test(self, test_seq, axis, gcmd): + reactor = self.printer.get_reactor() toolhead = self.printer.lookup_object('toolhead') X, Y, Z, E = toolhead.get_position() - sign = 1. - freq = self.freq_start # Override maximum acceleration and acceleration to # deceleration based on the maximum test frequency systime = self.printer.get_reactor().monotonic() toolhead_info = toolhead.get_status(systime) old_max_accel = toolhead_info['max_accel'] old_minimum_cruise_ratio = toolhead_info['minimum_cruise_ratio'] - max_accel = self.freq_end * self.accel_per_hz + max_accel = max([abs(a) for _, a in test_seq]) self.gcode.run_script_from_command( "SET_VELOCITY_LIMIT ACCEL=%.3f MINIMUM_CRUISE_RATIO=0" % (max_accel,)) @@ -88,24 +109,46 @@ def run_test(self, axis, gcmd): gcmd.respond_info("Disabled [input_shaper] for resonance testing") else: input_shaper = None - gcmd.respond_info("Testing frequency %.0f Hz" % (freq,)) - while freq <= self.freq_end + 0.000001: - t_seg = .25 / freq - accel = self.accel_per_hz * freq - max_v = accel * t_seg + last_v = last_t = last_accel = 0. + msg_rate = 1. / (bulk_sensor.BATCH_INTERVAL - 0.05) + total_time = test_seq[-1][0] + for next_t, accel in test_seq: + t_seg = next_t - last_t toolhead.cmd_M204(self.gcode.create_gcode_command( - "M204", "M204", {"S": accel})) - L = .5 * accel * t_seg**2 - dX, dY = axis.get_point(L) - nX = X + sign * dX - nY = Y + sign * dY - toolhead.move([nX, nY, Z, E], max_v) - toolhead.move([X, Y, Z, E], max_v) - sign = -sign - old_freq = freq - freq += 2. * t_seg * self.hz_per_sec - if math.floor(freq) > math.floor(old_freq): - gcmd.respond_info("Testing frequency %.0f Hz" % (freq,)) + "M204", "M204", {"S": abs(accel)})) + v = last_v + accel * t_seg + if abs(v) < 0.000001: + v = 0. + last_v2 = last_v * last_v + v2 = v * 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 + 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)) + else: + toolhead.move([nX, nY, Z, E], max(abs(v), abs(last_v)), last_v2) + if math.floor(next_t * msg_rate) > math.floor(last_t * msg_rate): + gcmd.respond_info("Testing %.1f%%.." % ( + next_t / total_time * 100.,)) + reactor.pause(reactor.monotonic() + 0.01) + X, Y = nX, nY + last_t = next_t + last_v = v + last_accel = accel + if last_v: + d_decel = -.5 * last_v2 / max_accel + decel_X, decel_Y = axis.get_point(d_decel) + toolhead.cmd_M204(self.gcode.create_gcode_command( + "M204", "M204", {"S": max_accel})) + toolhead.move([X + decel_X, Y + decel_Y, Z, E], abs(last_v)) # Restore the original acceleration values self.gcode.run_script_from_command( "SET_VELOCITY_LIMIT ACCEL=%.3f MINIMUM_CRUISE_RATIO=%.3f" @@ -114,14 +157,13 @@ def run_test(self, axis, gcmd): if input_shaper is not None: input_shaper.enable_shaping() gcmd.respond_info("Re-enabled [input_shaper]") - def get_max_freq(self): - return self.freq_end class ResonanceTester: def __init__(self, config): self.printer = config.get_printer() self.move_speed = config.getfloat('move_speed', 50., above=0.) - self.test = VibrationPulseTest(config) + self.generator = VibrationPulseTestGenerator(config) + self.executor = ResonanceTestExecutor(config) if not config.get('accel_chip_x', None): self.accel_chip_names = [('xy', config.get('accel_chip').strip())] else: @@ -131,6 +173,8 @@ def __init__(self, config): if self.accel_chip_names[0][1] == self.accel_chip_names[1][1]: self.accel_chip_names = [('xy', self.accel_chip_names[0][1])] self.max_smoothing = config.getfloat('max_smoothing', None, minval=0.05) + self.probe_points = config.getlists('probe_points', seps=(',', '\n'), + parser=float, count=3) self.gcode = self.printer.lookup_object('gcode') self.gcode.register_command("MEASURE_AXES_NOISE", @@ -154,12 +198,9 @@ def _run_test(self, gcmd, axes, helper, raw_name_suffix=None, toolhead = self.printer.lookup_object('toolhead') calibration_data = {axis: None for axis in axes} - self.test.prepare_test(gcmd) + self.generator.prepare_test(gcmd) - if test_point is not None: - test_points = [test_point] - else: - test_points = self.test.get_start_test_points() + test_points = [test_point] if test_point else self.probe_points for point in test_points: toolhead.manual_move(point, self.move_speed) @@ -184,7 +225,8 @@ def _run_test(self, gcmd, axes, helper, raw_name_suffix=None, raw_values.append((axis, aclient, chip.name)) # Generate moves - self.test.run_test(axis, gcmd) + test_seq = self.generator.gen_test() + self.executor.run_test(test_seq, axis, gcmd) for chip_axis, aclient, chip_name in raw_values: aclient.finish_measurements() if raw_name_suffix is not None: @@ -220,7 +262,7 @@ def _parse_chips(self, accel_chips): parsed_chips.append(chip) return parsed_chips def _get_max_calibration_freq(self): - return 1.5 * self.test.get_max_freq() + return 1.5 * self.generator.get_max_freq() cmd_TEST_RESONANCES_help = ("Runs the resonance test for a specifed axis") def cmd_TEST_RESONANCES(self, gcmd): # Parse parameters diff --git a/klippy/toolhead.py b/klippy/toolhead.py index 242bce0d61bd..b91ef59d2391 100644 --- a/klippy/toolhead.py +++ b/klippy/toolhead.py @@ -47,6 +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 def limit_speed(self, speed, accel): speed2 = speed**2 if speed2 < self.max_cruise_v2: @@ -55,6 +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 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]) @@ -75,7 +78,7 @@ def calc_junction(self, prev_move): # Apply limits 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) + self.junction_v2, prev_move.max_start_v2 + prev_move.delta_v2) if 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) @@ -463,7 +466,7 @@ 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): + def move(self, newpos, speed, max_junction_v2=None): move = Move(self, self.commanded_pos, newpos, speed) if not move.move_d: return @@ -471,6 +474,8 @@ def move(self, newpos, speed): 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: