1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
|
# A utility class to test resonances of the printer
#
# Copyright (C) 2020 Dmitry Butyugin <dmbutyugin@google.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math, os, time
from . import shaper_calibrate
def _parse_probe_points(config):
points = config.get('probe_points').split('\n')
try:
points = [line.split(',', 2) for line in points if line.strip()]
return [[float(coord.strip()) for coord in p] for p in points]
except:
raise config.error("Unable to parse probe_points in %s" % (
config.get_name()))
class VibrationPulseTest:
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=200.)
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 = _parse_probe_points(config)
def get_supported_axes(self):
return ['x', 'y']
def get_start_test_points(self):
return self.probe_points
def prepare_test(self, toolhead, 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=200.)
self.hz_per_sec = gcmd.get_float("HZ_PER_SEC", self.hz_per_sec,
above=0., maxval=2.)
# Attempt to adjust maximum acceleration and acceleration to
# deceleration based on the maximum test frequency.
max_accel = self.freq_end * self.accel_per_hz
toolhead.cmd_SET_VELOCITY_LIMIT(self.gcode.create_gcode_command(
"SET_VELOCITY_LIMIT", "SET_VELOCITY_LIMIT",
{"ACCEL": max_accel, "ACCEL_TO_DECEL": max_accel}))
def run_test(self, toolhead, axis, gcmd):
X, Y, Z, E = toolhead.get_position()
if axis not in self.get_supported_axes():
raise gcmd.error("Test axis '%s' is not supported", axis)
vib_dir = (1, 0) if axis == 'x' else (0., 1.)
sign = 1.
freq = self.freq_start
input_shaper = self.printer.lookup_object('input_shaper', None)
if input_shaper is not None and not gcmd.get_int('INPUT_SHAPING', 0):
input_shaper.disable_shaping()
gcmd.respond_info("Disabled [input_shaper] for resonance testing")
else:
input_shaper = None
gcmd.respond_info("Testing frequency %.0f Hz" % (freq,))
_, max_accel = toolhead.get_max_velocity()
while freq <= self.freq_end + 0.000001:
t_seg = .25 / freq
accel = min(self.accel_per_hz * freq, max_accel)
V = accel * t_seg
toolhead.cmd_M204(self.gcode.create_gcode_command(
"M204", "M204", {"S": accel}))
L = .5 * accel * t_seg**2
nX = X + sign * vib_dir[0] * L
nY = Y + sign * vib_dir[1] * L
toolhead.move([nX, nY, Z, E], V)
toolhead.move([X, Y, Z, E], 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,))
# Restore input shaper if it was disabled for resonance testing
if input_shaper is not None:
input_shaper.enable_shaping()
gcmd.respond_info("Re-enabled [input_shaper]")
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)
if not config.get('accel_chip_x', None):
self.accel_chip_names = [('xy', config.get('accel_chip').strip())]
else:
self.accel_chip_names = [
('x', config.get('accel_chip_x').strip()),
('y', config.get('accel_chip_y').strip())]
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.gcode = self.printer.lookup_object('gcode')
self.gcode.register_command("MEASURE_AXES_NOISE",
self.cmd_MEASURE_AXES_NOISE)
self.gcode.register_command("TEST_RESONANCES",
self.cmd_TEST_RESONANCES)
self.gcode.register_command("SHAPER_CALIBRATE",
self.cmd_SHAPER_CALIBRATE)
self.printer.register_event_handler("klippy:connect", self.connect)
def connect(self):
self.accel_chips = [
(axis, self.printer.lookup_object(chip_name))
for axis, chip_name in self.accel_chip_names]
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(toolhead, gcmd)
test_points = self.test.get_start_test_points()
for point in test_points:
toolhead.manual_move(point, self.move_speed)
if len(test_points) > 1:
gcmd.respond_info(
"Probing point (%.3f, %.3f, %.3f)" % tuple(point))
for axis in axes:
toolhead.wait_moves()
toolhead.dwell(0.500)
if len(axes) > 1:
gcmd.respond_info("Testing axis %s" % axis.upper())
for chip_axis, chip in self.accel_chips:
if axis in chip_axis or chip_axis in axis:
chip.start_measurements()
# Generate moves
self.test.run_test(toolhead, axis, gcmd)
raw_values = []
for chip_axis, chip in self.accel_chips:
if axis in chip_axis or chip_axis in axis:
results = chip.finish_measurements()
if raw_name_suffix is not None:
raw_name = self.get_filename(
'raw_data', raw_name_suffix, axis,
point if len(test_points) > 1 else None)
results.write_to_file(raw_name)
gcmd.respond_info(
"Writing raw accelerometer data to "
"%s file" % (raw_name,))
raw_values.append((chip_axis, results))
gcmd.respond_info("%s-axis accelerometer stats: %s" % (
chip_axis, results.get_stats(),))
if helper is None:
continue
for chip_axis, chip_values in raw_values:
if not chip_values:
raise gcmd.error(
"%s-axis accelerometer measured no data" % (
chip_axis,))
new_data = helper.process_accelerometer_data(chip_values)
if calibration_data[axis] is None:
calibration_data[axis] = new_data
else:
calibration_data[axis].add_data(new_data)
return calibration_data
def cmd_TEST_RESONANCES(self, gcmd):
# Parse parameters
if len(self.test.get_supported_axes()) > 1:
axis = gcmd.get("AXIS").lower()
else:
axis = gcmd.get("AXIS", self.test.get_supported_axes()[0]).lower()
if axis not in self.test.get_supported_axes():
raise gcmd.error("Unsupported axis '%s'" % (axis,))
outputs = gcmd.get("OUTPUT", "resonances").lower().split(',')
for output in outputs:
if output not in ['resonances', 'raw_data']:
raise gcmd.error("Unsupported output '%s', only 'resonances'"
" and 'raw_data' are supported" % (output,))
if not outputs:
raise gcmd.error("No output specified, at least one of 'resonances'"
" or 'raw_data' must be set in OUTPUT parameter")
name_suffix = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not self.is_valid_name_suffix(name_suffix):
raise gcmd.error("Invalid NAME parameter")
csv_output = 'resonances' in outputs
raw_output = 'raw_data' in outputs
# Setup calculation of resonances
if csv_output:
helper = shaper_calibrate.ShaperCalibrate(self.printer)
else:
helper = None
data = self._run_test(
gcmd, [axis], helper,
raw_name_suffix=name_suffix if raw_output else None)[axis]
if csv_output:
csv_name = self.save_calibration_data('resonances', name_suffix,
helper, axis, data)
gcmd.respond_info(
"Resonances data written to %s file" % (csv_name,))
def cmd_SHAPER_CALIBRATE(self, gcmd):
# Parse parameters
axis = gcmd.get("AXIS", None)
if not axis:
calibrate_axes = self.test.get_supported_axes()
elif axis.lower() not in self.test.get_supported_axes():
raise gcmd.error("Unsupported axis '%s'" % (axis,))
else:
calibrate_axes = [axis.lower()]
max_smoothing = gcmd.get_float(
"MAX_SMOOTHING", self.max_smoothing, minval=0.05)
name_suffix = gcmd.get("NAME", time.strftime("%Y%m%d_%H%M%S"))
if not self.is_valid_name_suffix(name_suffix):
raise gcmd.error("Invalid NAME parameter")
# Setup shaper calibration
helper = shaper_calibrate.ShaperCalibrate(self.printer)
calibration_data = self._run_test(gcmd, calibrate_axes, helper)
configfile = self.printer.lookup_object('configfile')
for axis in calibrate_axes:
gcmd.respond_info(
"Calculating the best input shaper parameters for %s axis"
% (axis,))
calibration_data[axis].normalize_to_frequencies()
best_shaper, all_shapers = helper.find_best_shaper(
calibration_data[axis], max_smoothing, gcmd.respond_info)
gcmd.respond_info(
"Recommended shaper_type_%s = %s, shaper_freq_%s = %.1f Hz"
% (axis, best_shaper.name, axis, best_shaper.freq))
helper.save_params(configfile, axis,
best_shaper.name, best_shaper.freq)
csv_name = self.save_calibration_data(
'calibration_data', name_suffix, helper, axis,
calibration_data[axis], all_shapers)
gcmd.respond_info(
"Shaper calibration data written to %s file" % (csv_name,))
gcmd.respond_info(
"The SAVE_CONFIG command will update the printer config file\n"
"with these parameters and restart the printer.")
def cmd_MEASURE_AXES_NOISE(self, gcmd):
meas_time = gcmd.get_float("MEAS_TIME", 2.)
for _, chip in self.accel_chips:
chip.start_measurements()
self.printer.lookup_object('toolhead').dwell(meas_time)
raw_values = [(axis, chip.finish_measurements())
for axis, chip in self.accel_chips]
helper = shaper_calibrate.ShaperCalibrate(self.printer)
for axis, raw_data in raw_values:
data = helper.process_accelerometer_data(raw_data)
vx = data.psd_x.mean()
vy = data.psd_y.mean()
vz = data.psd_z.mean()
gcmd.respond_info("Axes noise for %s-axis accelerometer: "
"%.6f (x), %.6f (y), %.6f (z)" % (
axis, vx, vy, vz))
def is_valid_name_suffix(self, name_suffix):
return name_suffix.replace('-', '').replace('_', '').isalnum()
def get_filename(self, base, name_suffix, axis=None, point=None):
name = base
if axis:
name += '_' + axis
if point:
name += "_%.3f_%.3f_%.3f" % (point[0], point[1], point[2])
name += '_' + name_suffix
return os.path.join("/tmp", name + ".csv")
def save_calibration_data(self, base_name, name_suffix, shaper_calibrate,
axis, calibration_data, all_shapers=None):
output = self.get_filename(base_name, name_suffix, axis)
shaper_calibrate.save_calibration_data(output, calibration_data,
all_shapers)
return output
def load_config(config):
return ResonanceTester(config)
|
