Skip to content

Commit

Permalink
resonance_tester: Factored out test generation code
Browse files Browse the repository at this point in the history 
Signed-off-by: Dmitry Butyugin <[email protected]>
  • Loading branch information
@dmbutyugin
dmbutyugin committed Oct 3, 2024
1 parent e03fd9f commit 74c4cbd
Show file tree
Hide file tree
Showing 2 changed files with 89 additions and 42 deletions.
122 changes: 82 additions & 40 deletions klippy/extras/resonance_tester.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down Expand Up @@ -45,40 +45,61 @@ 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.,
minval=self.min_freq, maxval=300.)
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,))
Expand All @@ -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"
Expand All @@ -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:
Expand All @@ -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",
Expand All @@ -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)
Expand All @@ -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:
Expand Down Expand Up @@ -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
Expand Down
9 changes: 7 additions & 2 deletions klippy/toolhead.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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])
Expand All @@ -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)
Expand Down Expand Up @@ -463,14 +466,16 @@ 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
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:
Expand Down

0 comments on commit 74c4cbd

Please sign in to comment.