From 006b5b7bdeecab69214810e62eef86479a6c6111 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 13 Sep 2022 21:29:31 +0200 Subject: [PATCH 01/35] pa_test: Utility class to print PA tower and virtual_sdcard refactoring Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 272 +++++++++++++++++++++ klippy/extras/sdcard_loop.py | 23 +- klippy/extras/virtual_sdcard.py | 413 ++++++++++++++++++-------------- 3 files changed, 513 insertions(+), 195 deletions(-) create mode 100644 klippy/extras/pa_test.py diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py new file mode 100644 index 000000000..3b6e20b32 --- /dev/null +++ b/klippy/extras/pa_test.py @@ -0,0 +1,272 @@ +# A utility class to run the Pressure Advance test +# +# Copyright (C) 2022 Dmitry Butyugin +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, math + +FIRST_LAYER_WIDTH_MULTIPLIER = 1.2 +DEFAULT_X_SIZE = 100.0 +DEFAULT_Y_SIZE = 50.0 +MAX_YX_SIZE_RATIO = 0.8 + +class PATest: + def __init__(self, config): + self.printer = config.get_printer() + self.printer.register_event_handler("klippy:connect", self._connect) + self.size_x = config.getfloat('size_x', 0., minval=0.) + self.size_y = config.getfloat('size_y', 0., minval=0.) + if self.size_x or self.size_y: + if self.size_x <= 0. or self.size_y <= 0: + raise config.error( + 'Both size_x and size_y must be set to positive values') + max_size_y = MAX_YX_SIZE_RATIO * self.size_x + if self.size_y > max_size_y: + raise config.error('Too large size_y value, maximum is %.3f' % ( + max_size_y,)) + self.height = config.getfloat('height', 50., above=0.) + self.origin_x = config.getfloat('origin_x', None) + self.origin_y = config.getfloat('origin_y', None) + self.layer_height = config.getfloat('layer_height', 0.2, above=0.) + self.first_layer_height = config.getfloat('first_layer_height', 0.2, + above=self.layer_height) + self.perimeters = config.getint('perimeters', 2, minval=1) + self.brim_width = config.getfloat('brim_width', 10., minval=2.) + self.slow_velocity = config.getfloat('slow_velocity', 20., above=0.) + self.fast_velocity = config.getfloat('fast_velocity', 80., + above=self.slow_velocity) + self.filament_diameter = config.getfloat('filament_diameter', 1.75, + above=0.) + # Register commands + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command('PRINT_PA_TOWER', + self.cmd_PRINT_PA_TOWER, + desc=self.cmd_PRINT_PA_TOWER_help) + self.progress = 0. + def _connect(self): + self.sdcard = self.printer.lookup_object('virtual_sdcard', None) + if self.sdcard is None: + raise self.printer.config_error( + 'virtual_sdcard must be enabled for pa_test module to work') + toolhead = self.printer.lookup_object('toolhead') + kin_status = toolhead.get_kinematics().get_status(eventtime=None) + self.origin_x = self.origin_x or .5 * (kin_status['axis_minimum'].x + + kin_status['axis_maximum'].x) + self.origin_y = self.origin_y or .5 * (kin_status['axis_minimum'].y + + kin_status['axis_maximum'].y) + if not self.size_x and not self.size_y: + # Restrict the size to a maximum of 80% of available space + self.size_x = min( + DEFAULT_X_SIZE, + 1.6 * (kin_status['axis_maximum'].x - self.origin_x), + 1.6 * (self.origin_x - kin_status['axis_minimum'].x)) + self.size_y = min( + DEFAULT_Y_SIZE, + 1.6 * (kin_status['axis_maximum'].y - self.origin_y), + 1.6 * (self.origin_x - kin_status['axis_minimum'].y)) + cmd_PRINT_PA_TOWER_help = "Start Pressure Advance Tower print" + def cmd_PRINT_PA_TOWER(self, gcmd): + if self.is_active(): + raise gcmd.error('PA tower is already printing. If you want to ' + 'pause or cancel the print, you must enable ' + '[pause_resume] module and call a corresponding ' + 'PAUSE/CANCEL_PRINT command.') + self.gcmd = gcmd + self.progress = 0. + self.sdcard.print_with_gcode_provider(self) + def handle_shutdown(self): + # Nothing to do, no resources to free + pass + def stats(self, eventtime): + return 'Printing PA tower' + def get_status(self, eventtime): + return { + 'file_path': self.get_name(), + 'progress': self.progress, + 'file_position': 0, + 'file_size': 0, + } + def is_active(self): + return self.sdcard.get_gcode_provider() == self + def get_name(self): + return 'PA tower' + def reset(self): + self.progress = 0. + def get_gcode(self): + gcmd = self.gcmd + nozzle = gcmd.get_float('NOZZLE') + + # Check extruder temperature + target_temp = gcmd.get_float('TARGET_TEMP') + toolhead = self.printer.lookup_object('toolhead') + extruder_heater = toolhead.get_extruder().get_heater() + systime = self.printer.get_reactor().monotonic() + heater_status = extruder_heater.get_status(systime) + if target_temp != heater_status['target']: + raise gcmd.error( + 'Extruder target temp %.1f does not match the expected ' + 'TARGET_TEMP=%.1f. Make sure to preheat the nozzle to ' + 'the expected temperature (e.g. with M109 gcode in a ' + 'gcode_macro prior to calling PRINT_PA_TOWER)' % ( + heater_status['target'], target_temp)) + + # Get tower params with overrides from the GCode command + origin_x = gcmd.get_float('ORIGIN_X', self.origin_x) + origin_y = gcmd.get_float('ORIGIN_Y', self.origin_y) + size_x = gcmd.get_float('SIZE_X', self.size_x) + size_y = gcmd.get_float('SIZE_Y', self.size_y, + maxval=MAX_YX_SIZE_RATIO*size_x) + perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) + slow_velocity = gcmd.get_float('SLOW_VELOCITY', self.slow_velocity, + above=0.) + fast_velocity = gcmd.get_float('FAST_VELOCITY', self.fast_velocity, + above=slow_velocity) + filament_diameter = gcmd.get_float('FILAMENT_DIAMETER', + self.filament_diameter, above=0.) + layer_height = gcmd.get_float('LAYER_HEIGHT', + self.layer_height, above=0.) + first_layer_height = gcmd.get_float('FIRST_LAYER_HEIGHT', + self.first_layer_height, + above=layer_height) + height = gcmd.get_float('HEIGHT', self.height, above=0.) + brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width, above=nozzle) + final_gcode_id = gcmd.get('FINAL_GCODE_ID', None) + + logging.info("Starting PA tower print of size %.3f x %.3f x %.3f mm" + " at (%.3f,%.3f)" % (size_x, size_y, height, + origin_x, origin_y)) + inner_size_x = size_x - 2 * nozzle * perimeters + inner_size_y = size_y - 2 * nozzle * perimeters + + def gen_brim(): + first_layer_width = nozzle * FIRST_LAYER_WIDTH_MULTIPLIER + extr_r = 4. * first_layer_height * first_layer_width / ( + math.pi * filament_diameter**2) + brim_x_offset = .5 * inner_size_x + brim_width + brim_y_offset = .5 * inner_size_y + brim_width + start_x = origin_x - brim_x_offset + start_y = origin_y - brim_y_offset + start_z = first_layer_height + yield 'G1 X%.3f Y%.3f Z%.3f F%.f' % ( + start_x, start_y, start_z, fast_velocity * 60.) + while brim_x_offset > .5 * (inner_size_x - first_layer_width): + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - brim_x_offset, origin_y + brim_y_offset, + 2. * brim_y_offset * extr_r, slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + brim_x_offset, origin_y + brim_y_offset, + 2. * brim_x_offset * extr_r, slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + brim_x_offset, origin_y - brim_y_offset, + 2. * brim_y_offset * extr_r, slow_velocity * 60.) + new_brim_x_offset = brim_x_offset - first_layer_width + new_brim_y_offset = brim_y_offset - first_layer_width + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - new_brim_x_offset, + origin_y - brim_y_offset, + (brim_x_offset + new_brim_x_offset) * extr_r, + slow_velocity * 60.) + brim_x_offset = new_brim_x_offset + brim_y_offset = new_brim_y_offset + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - brim_x_offset, origin_y + brim_y_offset, + 2. * brim_y_offset * extr_r, slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x, origin_y + brim_y_offset, + brim_x_offset * extr_r, slow_velocity * 60.) + self.progress = start_z / height + def gen_tower(): + z = first_layer_height + layer_height + x_switching_pos = .25 * size_x + extr_r = 4. * layer_height * nozzle / ( + math.pi * filament_diameter**2) + while z < height - 0.00000001: + perimeter_x_offset = .5 * inner_size_x + perimeter_y_offset = .5 * inner_size_y + # Move to the start of the perimeter + yield 'G1 X%.3f Y%.3f F%.f' % ( + origin_x, origin_y + perimeter_y_offset, + fast_velocity * 60.) + ## Move the nozzle up + yield 'G1 Z%.6f F%.f' % (z, slow_velocity * 60.) + for i in range(perimeters): + # Print the perimiter loop alternating velocities + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - x_switching_pos, + origin_y + perimeter_y_offset, + x_switching_pos * extr_r, slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, origin_y, + perimeter_y_offset * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y - perimeter_y_offset, + perimeter_y_offset * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - x_switching_pos, + origin_y - perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + x_switching_pos, + origin_y - perimeter_y_offset, + 2 * x_switching_pos * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + perimeter_x_offset, + origin_y - perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + perimeter_x_offset, origin_y, + perimeter_y_offset * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + perimeter_x_offset, + origin_y + perimeter_y_offset, + perimeter_y_offset * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + x_switching_pos, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x, origin_y + perimeter_y_offset, + x_switching_pos * extr_r, slow_velocity * 60.) + if i < perimeters - 1: + # Switch to the next perimeter + perimeter_x_offset += nozzle + perimeter_y_offset += nozzle + yield 'G1 X%.3f Y%.3f F%.f' % ( + origin_x, origin_y + perimeter_y_offset, + slow_velocity * 60.) + else: + # Hide the seam a bit + yield 'G1 X%.3f Y%.3f F%.f' % ( + origin_x - 2. * nozzle, + origin_y + perimeter_y_offset, + slow_velocity * 60.) + self.progress = z / height + z += layer_height + + yield 'M83' + yield 'G90' + for line in gen_brim(): + yield line + for line in gen_tower(): + yield line + if final_gcode_id is not None: + yield "UPDATE_DELAYED_GCODE ID='%s' DURATION=0.01" % ( + final_gcode_id,) + self.progress = 1.0 + +def load_config(config): + return PATest(config) diff --git a/klippy/extras/sdcard_loop.py b/klippy/extras/sdcard_loop.py index 6cc182ff9..95be2e23c 100644 --- a/klippy/extras/sdcard_loop.py +++ b/klippy/extras/sdcard_loop.py @@ -10,8 +10,9 @@ class SDCardLoop: def __init__(self, config): printer = config.get_printer() - self.sdcard = printer.load_object(config, "virtual_sdcard") - self.gcode = printer.lookup_object("gcode") + self.sdcard = sdcard = printer.load_object(config, "virtual_sdcard") + self.sdcard_gcode_provider = sdcard.get_virtual_sdcard_gcode_provider() + self.gcode = printer.lookup_object('gcode') self.gcode.register_command( "SDCARD_LOOP_BEGIN", self.cmd_SDCARD_LOOP_BEGIN, @@ -49,14 +50,17 @@ def cmd_SDCARD_LOOP_DESIST(self, gcmd): raise gcmd.error("Only permitted outside of a SD file.") def loop_begin(self, count): - if not self.sdcard.is_cmd_from_sd(): + if (not self.sdcard.is_cmd_from_sd() or + not self.sdcard_gcode_provider.is_active()): # Can only run inside of an SD file return False - self.loop_stack.append((count, self.sdcard.get_file_position())) + self.loop_stack.append( + (count, self.sdcard_gcode_provider.get_file_position())) return True def loop_end(self): - if not self.sdcard.is_cmd_from_sd(): + if (not self.sdcard.is_cmd_from_sd() or + not self.sdcard_gcode_provider.is_active()): # Can only run inside of an SD file return False # If the stack is empty, no need to skip back @@ -64,21 +68,22 @@ def loop_end(self): return True # Get iteration count and return position count, position = self.loop_stack.pop() - if count == 0: # Infinite loop - self.sdcard.set_file_position(position) + if count == 0: # Infinite loop + self.sdcard_gcode_provider.set_file_position(position) self.loop_stack.append((0, position)) elif count == 1: # Last repeat # Nothing to do pass else: # At the next opportunity, seek back to the start of the loop - self.sdcard.set_file_position(position) + self.sdcard_gcode_provider.set_file_position(position) # Decrement the count by 1, and add the position back to the stack self.loop_stack.append((count - 1, position)) return True def loop_desist(self): - if self.sdcard.is_cmd_from_sd(): + if (self.sdcard.is_cmd_from_sd() and + self.sdcard_gcode_provider.is_active()): # Can only run outside of an SD file return False logging.info("Desisting existing SD loops") diff --git a/klippy/extras/virtual_sdcard.py b/klippy/extras/virtual_sdcard.py index 0f8bc78d0..4dceab08e 100644 --- a/klippy/extras/virtual_sdcard.py +++ b/klippy/extras/virtual_sdcard.py @@ -8,49 +8,26 @@ VALID_GCODE_EXTS = ["gcode", "g", "gco"] -class VirtualSD: +class VirtualSDGCodeProvider: def __init__(self, config): self.printer = config.get_printer() - self.printer.register_event_handler( - "klippy:shutdown", self.handle_shutdown - ) + self.reactor = self.printer.get_reactor() # sdcard state sd = config.get("path") self.sdcard_dirname = os.path.normpath(os.path.expanduser(sd)) + self.filename = '' self.current_file = None self.file_position = self.file_size = 0 - # Print Stat Tracking - self.print_stats = self.printer.load_object(config, "print_stats") - # Work timer - self.reactor = self.printer.get_reactor() - self.must_pause_work = self.cmd_from_sd = False self.next_file_position = 0 - self.work_timer = None - # Error handling - gcode_macro = self.printer.load_object(config, "gcode_macro") - self.on_error_gcode = gcode_macro.load_template( - config, "on_error_gcode", "" - ) # Register commands - self.gcode = self.printer.lookup_object("gcode") - for cmd in ["M20", "M21", "M23", "M24", "M25", "M26", "M27"]: - self.gcode.register_command(cmd, getattr(self, "cmd_" + cmd)) - for cmd in ["M28", "M29", "M30"]: + self.gcode = self.printer.lookup_object('gcode') + for cmd in ['M20', 'M21', 'M26', 'M27']: + self.gcode.register_command(cmd, getattr(self, 'cmd_' + cmd)) + for cmd in ['M28', 'M29', 'M30']: self.gcode.register_command(cmd, self.cmd_error) - self.gcode.register_command( - "SDCARD_RESET_FILE", - self.cmd_SDCARD_RESET_FILE, - desc=self.cmd_SDCARD_RESET_FILE_help, - ) - self.gcode.register_command( - "SDCARD_PRINT_FILE", - self.cmd_SDCARD_PRINT_FILE, - desc=self.cmd_SDCARD_PRINT_FILE_help, - ) - + # Generic methods of GCode provider def handle_shutdown(self): - if self.work_timer is not None: - self.must_pause_work = True + if self.current_file is not None: try: readpos = max(self.file_position - 1024, 0) readcount = self.file_position - readpos @@ -68,10 +45,70 @@ def handle_shutdown(self): ) def stats(self, eventtime): - if self.work_timer is None: - return False, "" - return True, "sd_pos=%d" % (self.file_position,) - + return "sd_pos=%d" % (self.file_position,) + def get_status(self, eventtime): + return { + 'file_path': self.file_path(), + 'progress': self.progress(), + 'file_position': self.file_position, + 'file_size': self.file_size, + } + def is_active(self): + return self.current_file is not None + def get_name(self): + return self.filename + def reset(self): + if self.current_file is not None: + self.current_file.close() + self.current_file = None + self.filename = '' + self.file_position = self.file_size = 0 + def get_gcode(self): + logging.info("Starting SD card print (position %d)", self.file_position) + try: + self.current_file.seek(self.file_position) + except: + logging.exception("virtual_sdcard seek") + return + partial_input = "" + lines = [] + while True: + if not lines: + # Read more data + try: + data = self.current_file.read(8192) + except: + logging.exception("virtual_sdcard read") + break + if not data: + # End of file + self.current_file.close() + self.current_file = None + logging.info("Finished SD card print") + self.gcode.respond_raw("Done printing file") + break + lines = data.split('\n') + lines[0] = partial_input + lines[0] + partial_input = lines.pop() + lines.reverse() + self.reactor.pause(self.reactor.NOW) + continue + line = lines.pop() + next_file_position = self.file_position + len(line) + 1 + self.next_file_position = next_file_position + yield line + self.file_position = self.next_file_position + # Do we need to skip around? + if self.next_file_position != next_file_position: + try: + self.current_file.seek(self.file_position) + except: + logging.exception("virtual_sdcard seek") + return + lines = [] + partial_input = "" + logging.info("Exiting SD card print (position %d)", self.file_position) + # Virtual SD Card file management def get_file_list(self, check_subdirs=False): if check_subdirs: flist = [] @@ -100,16 +137,6 @@ def get_file_list(self, check_subdirs=False): except: logging.exception("virtual_sdcard get_file_list") raise self.gcode.error("Unable to get file list") - - def get_status(self, eventtime): - return { - "file_path": self.file_path(), - "progress": self.progress(), - "is_active": self.is_active(), - "file_position": self.file_position, - "file_size": self.file_size, - } - def file_path(self): if self.current_file: return self.current_file.name @@ -119,115 +146,175 @@ def progress(self): if self.file_size: return float(self.file_position) / self.file_size else: - return 0.0 + return 0. + def load_file(self, gcmd, filename, check_subdirs=False): + files = self.get_file_list(check_subdirs) + flist = [f[0] for f in files] + files_by_lower = {fname.lower(): fname for fname, fsize in files} + fname = filename + try: + if fname not in flist: + fname = files_by_lower[fname.lower()] + fname = os.path.join(self.sdcard_dirname, fname) + f = io.open(fname, "r", newline="") + f.seek(0, os.SEEK_END) + fsize = f.tell() + f.seek(0) + except: + logging.exception("virtual_sdcard file open") + raise gcmd.error("Unable to open file") + gcmd.respond_raw("File opened:%s Size:%d" % (filename, fsize)) + gcmd.respond_raw("File selected") + self.current_file = f + self.file_position = 0 + self.file_size = fsize + self.filename = filename + def get_file_position(self): + return self.next_file_position + def set_file_position(self, pos): + self.next_file_position = pos + # G-Code commands + def cmd_error(self, gcmd): + raise gcmd.error("SD write not supported") + def cmd_M20(self, gcmd): + # List SD card + files = self.get_file_list() + gcmd.respond_raw("Begin file list") + for fname, fsize in files: + gcmd.respond_raw("%s %d" % (fname, fsize)) + gcmd.respond_raw("End file list") + def cmd_M21(self, gcmd): + # Initialize SD card + gcmd.respond_raw("SD card ok") + def cmd_M26(self, gcmd): + # Set SD position + if not self.is_active(): + gcmd.respond_raw("Not printing from SD card.") + pos = gcmd.get_int('S', minval=0) + self.set_file_position(pos) + def cmd_M27(self, gcmd): + # Report SD print status + if not self.is_active(): + gcmd.respond_raw("Not printing from SD card.") + return + gcmd.respond_raw("SD printing byte %d/%d" + % (self.file_position, self.file_size)) +class VirtualSD: + def __init__(self, config): + self.printer = config.get_printer() + self.printer.register_event_handler("klippy:shutdown", + self.handle_shutdown) + # Print Stat Tracking + self.print_stats = self.printer.load_object(config, 'print_stats') + # sdcard state + self.virtualsd_gcode_provider = VirtualSDGCodeProvider(config) + self.gcode_provider = None + # Work timer + self.reactor = self.printer.get_reactor() + self.must_pause_work = self.cmd_from_sd = False + self.work_timer = None + # Error handling + gcode_macro = self.printer.load_object(config, 'gcode_macro') + self.on_error_gcode = gcode_macro.load_template( + config, 'on_error_gcode', '') + # Register commands + self.gcode = self.printer.lookup_object('gcode') + for cmd in ['M23', 'M24', 'M25']: + self.gcode.register_command(cmd, getattr(self, 'cmd_' + cmd)) + self.gcode.register_command( + "SDCARD_RESET_FILE", self.cmd_SDCARD_RESET_FILE, + desc=self.cmd_SDCARD_RESET_FILE_help) + self.gcode.register_command( + "SDCARD_PRINT_FILE", self.cmd_SDCARD_PRINT_FILE, + desc=self.cmd_SDCARD_PRINT_FILE_help) + def handle_shutdown(self): + if self.work_timer is not None: + self.must_pause_work = True + self.gcode_provider.handle_shutdown() + def get_file_list(self, check_subdirs=False): + return self.virtualsd_gcode_provider.get_file_list(check_subdirs) + def stats(self, eventtime): + if self.work_timer is None: + return False, "" + return True, self.gcode_provider.stats(eventtime) + def get_status(self, eventtime): + sts = {'is_active': self.is_active()} + if self.gcode_provider: + sts.update(self.gcode_provider.get_status(eventtime)) + else: + sts.update(self.virtualsd_gcode_provider.get_status(eventtime)) + return sts def is_active(self): return self.work_timer is not None - def do_pause(self): if self.work_timer is not None: self.must_pause_work = True while self.work_timer is not None and not self.cmd_from_sd: - self.reactor.pause(self.reactor.monotonic() + 0.001) - + self.reactor.pause(self.reactor.monotonic() + .001) def do_resume(self): if self.work_timer is not None: raise self.gcode.error("SD busy") self.must_pause_work = False self.work_timer = self.reactor.register_timer( - self.work_handler, self.reactor.NOW - ) - + self.work_handler, self.reactor.NOW) def do_cancel(self): - if self.current_file is not None: + if self.gcode_provider is not None: self.do_pause() - self.current_file.close() - self.current_file = None + self.gcode_provider.reset() self.print_stats.note_cancel() - self.file_position = self.file_size = 0 - + self.gcode_provider = None + def _set_gcode_provider(self, gcode_provider): + if self.gcode_provider is not None: + raise self.gcode.error( + 'Print is active when resetting GCode provider') + self.gcode_provider = gcode_provider + self.print_stats.set_current_file(gcode_provider.get_name()) + self.gcode_lines = gcode_provider.get_gcode() + self.current_line = '' + def print_with_gcode_provider(self, gcode_provider): + self._reset_print() + self._set_gcode_provider(gcode_provider) + self.do_resume() # G-Code commands - def cmd_error(self, gcmd): - raise gcmd.error("SD write not supported") - - def _reset_file(self): - if self.current_file is not None: + def _reset_print(self): + if self.gcode_provider is not None: self.do_pause() - self.current_file.close() - self.current_file = None - self.file_position = self.file_size = 0 + self.gcode_provider.reset() + self.gcode_provider = None self.print_stats.reset() self.printer.send_event("virtual_sdcard:reset_file") - - cmd_SDCARD_RESET_FILE_help = ( - "Clears a loaded SD File. Stops the print " "if necessary" - ) - + cmd_SDCARD_RESET_FILE_help = "Clears a loaded SD File. Stops the print "\ + "if necessary" def cmd_SDCARD_RESET_FILE(self, gcmd): if self.cmd_from_sd: - raise gcmd.error("SDCARD_RESET_FILE cannot be run from the sdcard") - self._reset_file() - - cmd_SDCARD_PRINT_FILE_help = ( - "Loads a SD file and starts the print. May " + raise gcmd.error( + "SDCARD_RESET_FILE cannot be run from the sdcard") + self._reset_print() + cmd_SDCARD_PRINT_FILE_help = "Loads a SD file and starts the print. May "\ "include files in subdirectories." - ) - def cmd_SDCARD_PRINT_FILE(self, gcmd): if self.work_timer is not None: raise gcmd.error("SD busy") - self._reset_file() + self._reset_print() filename = gcmd.get("FILENAME") - if filename[0] == "/": + if filename[0] == '/': filename = filename[1:] - self._load_file(gcmd, filename, check_subdirs=True) + self.virtualsd_gcode_provider.load_file(gcmd, filename, + check_subdirs=True) + self._set_gcode_provider(self.virtualsd_gcode_provider) self.do_resume() - - def cmd_M20(self, gcmd): - # List SD card - files = self.get_file_list() - gcmd.respond_raw("Begin file list") - for fname, fsize in files: - gcmd.respond_raw("%s %d" % (fname, fsize)) - gcmd.respond_raw("End file list") - - def cmd_M21(self, gcmd): - # Initialize SD card - gcmd.respond_raw("SD card ok") - def cmd_M23(self, gcmd): # Select SD file if self.work_timer is not None: raise gcmd.error("SD busy") - self._reset_file() + self._reset_print() filename = gcmd.get_raw_command_parameters().strip() - if filename.startswith("/"): + if filename.startswith('/'): filename = filename[1:] - self._load_file(gcmd, filename) - - def _load_file(self, gcmd, filename, check_subdirs=False): - files = self.get_file_list(check_subdirs) - flist = [f[0] for f in files] - files_by_lower = {fname.lower(): fname for fname, fsize in files} - fname = filename - try: - if fname not in flist: - fname = files_by_lower[fname.lower()] - fname = os.path.join(self.sdcard_dirname, fname) - f = io.open(fname, "r", newline="") - f.seek(0, os.SEEK_END) - fsize = f.tell() - f.seek(0) - except: - logging.exception("virtual_sdcard file open") - raise gcmd.error("Unable to open file") - gcmd.respond_raw("File opened:%s Size:%d" % (filename, fsize)) - gcmd.respond_raw("File selected") - self.current_file = f - self.file_position = 0 - self.file_size = fsize - self.print_stats.set_current_file(filename) - + self.virtualsd_gcode_provider.load_file(gcmd, filename, + check_subdirs=True) + self._set_gcode_provider(self.virtualsd_gcode_provider) def cmd_M24(self, gcmd): # Start/resume SD print self.do_resume() @@ -235,77 +322,40 @@ def cmd_M24(self, gcmd): def cmd_M25(self, gcmd): # Pause SD print self.do_pause() - - def cmd_M26(self, gcmd): - # Set SD position - if self.work_timer is not None: - raise gcmd.error("SD busy") - pos = gcmd.get_int("S", minval=0) - self.file_position = pos - - def cmd_M27(self, gcmd): - # Report SD print status - if self.current_file is None: - gcmd.respond_raw("Not SD printing.") - return - gcmd.respond_raw( - "SD printing byte %d/%d" % (self.file_position, self.file_size) - ) - - def get_file_position(self): - return self.next_file_position - - def set_file_position(self, pos): - self.next_file_position = pos - + def get_virtual_sdcard_gcode_provider(self): + return self.virtualsd_gcode_provider + def get_gcode_provider(self): + return self.gcode_provider def is_cmd_from_sd(self): return self.cmd_from_sd # Background work timer def work_handler(self, eventtime): - logging.info("Starting SD card print (position %d)", self.file_position) self.reactor.unregister_timer(self.work_timer) - try: - self.current_file.seek(self.file_position) - except: - logging.exception("virtual_sdcard seek") - self.work_timer = None - return self.reactor.NEVER self.print_stats.note_start() gcode_mutex = self.gcode.get_mutex() - partial_input = "" - lines = [] error_message = None while not self.must_pause_work: - if not lines: - # Read more data + if not self.current_line: try: - data = self.current_file.read(8192) - except: - logging.exception("virtual_sdcard read") + self.current_line = next(self.gcode_lines) + except self.gcode.error as e: + error_message = str(e) + try: + self.gcode.run_script(self.on_error_gcode.render()) + except: + logging.exception("virtual_sdcard on_error") break - if not data: - # End of file - self.current_file.close() - self.current_file = None - logging.info("Finished SD card print") - self.gcode.respond_raw("Done printing file") + except StopIteration: + self.gcode_provider = None break - lines = data.split("\n") - lines[0] = partial_input + lines[0] - partial_input = lines.pop() - lines.reverse() - self.reactor.pause(self.reactor.NOW) - continue # Pause if any other request is pending in the gcode class if gcode_mutex.test(): self.reactor.pause(self.reactor.monotonic() + 0.100) continue # Dispatch command self.cmd_from_sd = True - line = lines.pop() - next_file_position = self.file_position + len(line) + 1 - self.next_file_position = next_file_position + line = self.current_line try: self.gcode.run_script(line) except self.gcode.error as e: @@ -319,23 +369,14 @@ def work_handler(self, eventtime): logging.exception("virtual_sdcard dispatch") break self.cmd_from_sd = False - self.file_position = self.next_file_position - # Do we need to skip around? - if self.next_file_position != next_file_position: - try: - self.current_file.seek(self.file_position) - except: - logging.exception("virtual_sdcard seek") - self.work_timer = None - return self.reactor.NEVER - lines = [] - partial_input = "" - logging.info("Exiting SD card print (position %d)", self.file_position) + self.current_line = '' self.work_timer = None self.cmd_from_sd = False if error_message is not None: self.print_stats.note_error(error_message) - elif self.current_file is not None: + self.gcode.respond_raw(error_message) + self.gcode_provider = None + elif self.gcode_provider is not None: self.print_stats.note_pause() else: self.print_stats.note_complete() From 4c330b4f3a04beeb97a9277c1c66f76f3c15079a Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 16 Oct 2022 22:30:54 +0200 Subject: [PATCH 02/35] ringing_test: Utility class to print ringing tower Signed-off-by: Dmitry Butyugin --- klippy/extras/ringing_test.py | 355 ++++++++++++++++++++++++++++++++++ 1 file changed, 355 insertions(+) create mode 100644 klippy/extras/ringing_test.py diff --git a/klippy/extras/ringing_test.py b/klippy/extras/ringing_test.py new file mode 100644 index 000000000..d71b6952e --- /dev/null +++ b/klippy/extras/ringing_test.py @@ -0,0 +1,355 @@ +# A utility class to print the ringing (ghosting) test +# +# Copyright (C) 2022 Dmitry Butyugin +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import logging, math + +FIRST_LAYER_WIDTH_MULTIPLIER = 1.1 +DEFAULT_SIZE = 100.0 +TAN_TEST_ANGLE = 1. +DEFAULT_NOTCH_OFFSET_RATIO = .275 +INFINITE_ACCEL = 1000000.0 +LETTER_BAND_PART = 0.82 + +class RingingTest: + def __init__(self, config): + self.printer = config.get_printer() + self.printer.register_event_handler("klippy:connect", self._connect) + self.size = config.getfloat('size', 0., minval=50.) + self.height = config.getfloat('height', 60., above=0.) + self.band = config.getfloat('band', 5., above=0.) + self.notch = config.getfloat('notch', 1., above=0.) + self.notch_offset = config.getfloat('notch_offset', 0.) + self.velocity = config.getfloat('velocity', 80., above=0.) + self.accel_start = config.getfloat('accel_start', 1500., above=0.) + self.accel_step = config.getfloat('accel_step', 500., above=0.) + self.center_x = config.getfloat('center_x', None) + self.center_y = config.getfloat('center_y', None) + self.layer_height = config.getfloat('layer_height', 0.2, above=0.) + self.first_layer_height = config.getfloat('first_layer_height', 0.2, + above=self.layer_height) + self.perimeters = config.getint('perimeters', 2, minval=1) + self.brim_width = config.getfloat('brim_width', 10., + minval=self.notch+2.) + self.brim_velocity = config.getfloat('brim_velocity', 30., above=0.) + self.filament_diameter = config.getfloat('filament_diameter', 1.75, + above=0.) + self.deceleration_points = config.getint('deceleration_points', 100, + minval=10) + # Register commands + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command('PRINT_RINGING_TOWER', + self.cmd_PRINT_RINGING_TOWER, + desc=self.cmd_PRINT_RINGING_TOWER_help) + self.progress = 0. + def _connect(self): + self.sdcard = self.printer.lookup_object('virtual_sdcard', None) + if self.sdcard is None: + raise self.printer.config_error( + 'virtual_sdcard must be enabled for pa_test module to work') + toolhead = self.printer.lookup_object('toolhead') + kin_status = toolhead.get_kinematics().get_status(eventtime=None) + self.center_x = self.center_x or .5 * (kin_status['axis_minimum'].x + + kin_status['axis_maximum'].x) + self.center_y = self.center_y or .5 * (kin_status['axis_minimum'].y + + kin_status['axis_maximum'].y) + if not self.size: + # Restrict the size to a maximum of 80% of available space + self.size = min( + DEFAULT_SIZE, + 1.6 * (kin_status['axis_maximum'].x - self.center_x), + 1.6 * (self.center_x - kin_status['axis_minimum'].x)) + if self.notch_offset: + raise self.printer.config_error( + 'notch_offset must not be set if size is not provided') + self.notch_offset = self.size * DEFAULT_NOTCH_OFFSET_RATIO + else: + if not self.notch_offset: + self.notch_offset = self.size * DEFAULT_NOTCH_OFFSET_RATIO + if self.notch_offset < 2. or self.notch_offset > .5 * self.size: + raise self.printer.config_error( + 'notch_offset must not be in range [2.0, %.3f]' % ( + .5 * self.size,)) + + cmd_PRINT_RINGING_TOWER_help = "Start Ringing Tower print" + def cmd_PRINT_RINGING_TOWER(self, gcmd): + if self.is_active(): + raise gcmd.error('Ringing tower is already printing. If you want ' + 'to pause or cancel the print, you must enable ' + '[pause_resume] module and call a corresponding ' + 'PAUSE/CANCEL_PRINT command.') + self.gcmd = gcmd + self.progress = 0. + self.sdcard.print_with_gcode_provider(self) + def handle_shutdown(self): + # Nothing to do, no resources to free + pass + def stats(self, eventtime): + return 'Printing ringing tower' + def get_status(self, eventtime): + return { + 'file_path': self.get_name(), + 'progress': self.progress, + 'file_position': 0, + 'file_size': 0, + } + def is_active(self): + return self.sdcard.get_gcode_provider() == self + def get_name(self): + return 'Ringing tower' + def reset(self): + self.progress = 0. + def get_gcode(self): + gcmd = self.gcmd + nozzle = gcmd.get_float('NOZZLE') + line_width = nozzle * 1.2 + + # Get velocity and acceleration limits + toolhead = self.printer.lookup_object('toolhead') + systime = self.printer.get_reactor().monotonic() + toolhead_info = toolhead.get_status(systime) + old_max_accel = toolhead_info['max_accel'] + old_max_accel_to_decel = toolhead_info['max_accel_to_decel'] + old_max_velocity = toolhead_info['max_velocity'] + + # Get tower params with overrides from the GCode command + center_x = gcmd.get_float('CENTER_X', self.center_x) + center_y = gcmd.get_float('CENTER_Y', self.center_y) + size = gcmd.get_float('SIZE', self.size, minval=50.) + perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) + brim_velocity = gcmd.get_float('BRIM_VELOCITY', self.brim_velocity, + above=0.) + filament_diameter = gcmd.get_float('FILAMENT_DIAMETER', + self.filament_diameter, above=0.) + layer_height = gcmd.get_float('LAYER_HEIGHT', + self.layer_height, above=0.) + first_layer_height = gcmd.get_float('FIRST_LAYER_HEIGHT', + self.first_layer_height, + above=layer_height) + height = gcmd.get_float('HEIGHT', self.height, above=0.) + band = gcmd.get_float('BAND', self.band, above=0.) + notch = gcmd.get_float('NOTCH', self.notch, above=0.) + notch_offset = gcmd.get_float( + 'NOTCH_OFFSET', + self.notch_offset if size == self.size + else size * DEFAULT_NOTCH_OFFSET_RATIO, + above=2., maxval=.5*size) + velocity = gcmd.get_float('VELOCITY', self.velocity, above=0.) + accel_start = gcmd.get_float('ACCEL_START', self.accel_start, above=0.) + accel_step = gcmd.get_float('ACCEL_STEP', self.accel_step, above=0.) + brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width) + min_brim_width = (notch_offset - notch) / (1. + 1. / TAN_TEST_ANGLE) + brim_width = max(brim_width, + min_brim_width + perimeters * line_width + 1.) + final_gcode_id = gcmd.get('FINAL_GCODE_ID', None) + deceleration_points = gcmd.get_int('DECELERATION_POINTS', + self.deceleration_points, minval=10) + + logging.info("Starting ringing tower print of size %.3fx%.3fx%.3f mm" + " at (%.3f,%.3f)" % (size, size, height, + center_x, center_y)) + inner_size = size - 2 * (line_width * perimeters + notch) + inner_brim_size = inner_size - 2. * min_brim_width + + recipr_cos = math.sqrt(1. + TAN_TEST_ANGLE**2) + max_velocity = velocity * recipr_cos + if max_velocity > old_max_velocity: + yield 'SET_VELOCITY_LIMIT VELOCITY=%.3f' % (max_velocity,) + def gen_brim(): + first_layer_width = line_width * FIRST_LAYER_WIDTH_MULTIPLIER + extr_r = 4. * first_layer_height * first_layer_width / ( + math.pi * filament_diameter**2) + brim_offset = .5 * inner_brim_size + brim_width + start_x = center_x - brim_offset + start_y = center_y - brim_offset + start_z = first_layer_height + yield 'SET_VELOCITY_LIMIT ACCEL=%.6f ACCEL_TO_DECEL=%.6f' % ( + accel_start, .5 * accel_start) + yield 'G1 X%.3f Y%.3f Z%.3f F%.f' % ( + start_x, start_y, start_z, + max_velocity * 60.) + while brim_offset > .5 * inner_brim_size: + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + center_x - brim_offset, center_y + brim_offset, + 2. * brim_offset * extr_r, brim_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + center_x + brim_offset, center_y + brim_offset, + 2. * brim_offset * extr_r, brim_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + center_x + brim_offset, center_y - brim_offset, + 2. * brim_offset * extr_r, brim_velocity * 60.) + new_brim_offset = brim_offset - first_layer_width + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + center_x - new_brim_offset, + center_y - brim_offset, + (brim_offset + new_brim_offset) * extr_r, + brim_velocity * 60.) + brim_offset = new_brim_offset + self.progress = start_z / height + def gen_tower(): + prev_z = first_layer_height + z = first_layer_height + layer_height + extr_r = 4. * layer_height * line_width / ( + math.pi * filament_diameter**2) + letter_offset = .5 * (size - band) - notch_offset * TAN_TEST_ANGLE + while z < height - 0.00000001: + next_z = z + layer_height + band_part = math.fmod(z, band) / band + notch_pos = notch_offset - notch / (1. - math.sqrt(0.75)) * ( + 1. - math.sqrt(1. - (band_part - 0.5)**2)) + max_accel = accel_start + accel_step * math.floor(z / band) + v_y = velocity * TAN_TEST_ANGLE + t_y = v_y / max_accel + d_y = .5 * v_y * t_y + d_x = velocity * t_y + notch_other_side = (notch_pos - d_x) * TAN_TEST_ANGLE + d_y + perimeter_offset = .5 * inner_size + yield 'SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.3f' % ( + max_accel, .5 * max_accel) + for i in range(perimeters): + # Move to the start of the perimeter + next_y_offset = ( + perimeter_offset - notch_other_side + - (perimeter_offset - .5 * size) * TAN_TEST_ANGLE) + yield 'G1 X%.6f Y%.6f F%.3f' % ( + center_x - perimeter_offset, + center_y - next_y_offset, + velocity * 60.) + yield 'G1 Z%.6f' % (z,) + # Print the perimiter loop + for notch_axis in [(1, 0), (0, 1), (-1, 0), (0, -1)]: + other_axis = (notch_axis[1], -notch_axis[0]) + def rotated_G1(x, y, e, v): + return 'G1 X%.6f Y%.6f E%.9f F%.3f' % ( + center_x + notch_axis[0] * x + + other_axis[0] * y, + center_y + notch_axis[1] * x + + other_axis[1] * y, + e * extr_r, v * 60.) + # The extrusion flow of the lines at an agle is reduced + # by cos(angle) to maintain the correct spacing between + # the perimeters formed by those lines + yield rotated_G1( + notch_pos - d_x - 1.0 - .5 * size, + perimeter_offset - d_y - TAN_TEST_ANGLE, + (notch_pos - d_x - 1.0 - .5 * size + + perimeter_offset), + max_velocity) + yield ('SET_VELOCITY_LIMIT ACCEL=%.6f' + + ' ACCEL_TO_DECEL=%.6f') % ( + INFINITE_ACCEL, INFINITE_ACCEL) + yield rotated_G1( + notch_pos - d_x - .5 * size, + perimeter_offset - d_y, + 1., max_velocity) + old_x, old_y = d_x, d_y + for j in range(deceleration_points): + x = ((deceleration_points - j - 1) * d_x + / deceleration_points) + t = (d_x - x) / velocity + y = d_y - t * (v_y - .5 * max_accel * t) + d = math.sqrt((x - old_x)**2 + (y - old_y)**2) + v = velocity * d / (old_x - x) + yield rotated_G1( + notch_pos - x - .5 * size, + perimeter_offset - y, + (old_x - x), v) + old_x, old_y = x, y + yield ('SET_VELOCITY_LIMIT ACCEL=%.6f' + + ' ACCEL_TO_DECEL=%.6f') % ( + max_accel, .5 * max_accel) + if i < perimeters - 1 or (abs(band_part - .5) >= + .5 * LETTER_BAND_PART): + yield rotated_G1( + next_y_offset, perimeter_offset, + next_y_offset - notch_pos + .5 * size, + velocity) + continue + # Emboss a letter + print_serif = ( + max(math.fmod(next_z, band) / band - .5, + .5 - math.fmod(prev_z, band) / band) + >= .5 * LETTER_BAND_PART + and abs(band_part - .5) < .5 * LETTER_BAND_PART) + letter_width = abs(band_part - .5) * band + nozzle + if notch_axis[0] and band_part < .5: + # Bottom of Y letter + letter_width = nozzle + elem_size = nozzle + if print_serif: + letter_width += nozzle + elem_size += nozzle + letter_perimeter_offset = -.5 * nozzle + yield rotated_G1( + letter_offset - letter_width * .5, + perimeter_offset, + (letter_offset - letter_width * .5 + - notch_pos + .5 * size), + velocity) + yield rotated_G1( + letter_offset - letter_width * .5, + perimeter_offset - letter_perimeter_offset, + abs(letter_perimeter_offset), velocity) + if letter_width > 2. * elem_size: + yield rotated_G1( + (letter_offset - letter_width * .5 + + elem_size), + perimeter_offset - letter_perimeter_offset, + elem_size, velocity) + yield rotated_G1( + (letter_offset - letter_width * .5 + + elem_size), + perimeter_offset, + abs(letter_perimeter_offset), velocity) + yield rotated_G1( + (letter_offset + letter_width * .5 + - elem_size), + perimeter_offset, + letter_width - 2. * elem_size, velocity) + yield rotated_G1( + (letter_offset + letter_width * .5 + - elem_size), + perimeter_offset - letter_perimeter_offset, + abs(letter_perimeter_offset), velocity) + yield rotated_G1( + letter_offset + letter_width * .5, + perimeter_offset - letter_perimeter_offset, + elem_size, velocity) + else: + yield rotated_G1( + letter_offset + letter_width * .5, + perimeter_offset - letter_perimeter_offset, + letter_width, velocity) + yield rotated_G1( + letter_offset + letter_width * .5, + perimeter_offset, + abs(letter_perimeter_offset), velocity) + yield rotated_G1( + next_y_offset, + perimeter_offset, + (next_y_offset - letter_offset + - letter_width * .5), + velocity) + perimeter_offset += line_width + self.progress = z / height + prev_z, z = z, next_z + yield ('SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.f' + + ' VELOCITY=%.3f') % ( + old_max_accel, old_max_accel_to_decel, + old_max_velocity) + + yield 'M83' + yield 'G90' + yield 'M220 S100' + for line in gen_brim(): + yield line + for line in gen_tower(): + yield line + if final_gcode_id is not None: + yield "UPDATE_DELAYED_GCODE ID='%s' DURATION=0.01" % ( + final_gcode_id,) + self.progress = 1.0 + +def load_config(config): + return RingingTest(config) From 86a4bfdf90413ecf6cf976509a1251f0edf1fdd0 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 30 May 2023 19:25:47 +0200 Subject: [PATCH 03/35] virtual_sdcard: Fixed stats reporting in PA and ringing tests Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 6 ++++-- klippy/extras/ringing_test.py | 6 ++++-- klippy/extras/virtual_sdcard.py | 17 ++++++----------- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index 3b6e20b32..a1e6047cf 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -77,8 +77,10 @@ def cmd_PRINT_PA_TOWER(self, gcmd): def handle_shutdown(self): # Nothing to do, no resources to free pass - def stats(self, eventtime): - return 'Printing PA tower' + def get_stats(self, eventtime): + if not self.is_active(): + return False, '' + return True, 'Printing PA tower' def get_status(self, eventtime): return { 'file_path': self.get_name(), diff --git a/klippy/extras/ringing_test.py b/klippy/extras/ringing_test.py index d71b6952e..717210833 100644 --- a/klippy/extras/ringing_test.py +++ b/klippy/extras/ringing_test.py @@ -85,8 +85,10 @@ def cmd_PRINT_RINGING_TOWER(self, gcmd): def handle_shutdown(self): # Nothing to do, no resources to free pass - def stats(self, eventtime): - return 'Printing ringing tower' + def get_stats(self, eventtime): + if not self.is_active(): + return False, '' + return True, 'Printing ringing tower' def get_status(self, eventtime): return { 'file_path': self.get_name(), diff --git a/klippy/extras/virtual_sdcard.py b/klippy/extras/virtual_sdcard.py index 4dceab08e..fc1a6fc8f 100644 --- a/klippy/extras/virtual_sdcard.py +++ b/klippy/extras/virtual_sdcard.py @@ -36,16 +36,11 @@ def handle_shutdown(self): except: logging.exception("virtual_sdcard shutdown read") return - logging.info( - "Virtual sdcard (%d): %s\nUpcoming (%d): %s", - readpos, - repr(data[:readcount]), - self.file_position, - repr(data[readcount:]), - ) - - def stats(self, eventtime): - return "sd_pos=%d" % (self.file_position,) + logging.info("Virtual sdcard (%d): %s\nUpcoming (%d): %s", + readpos, repr(data[:readcount]), + self.file_position, repr(data[readcount:])) + def get_stats(self, eventtime): + return True, "sd_pos=%d" % (self.file_position,) def get_status(self, eventtime): return { 'file_path': self.file_path(), @@ -237,7 +232,7 @@ def get_file_list(self, check_subdirs=False): def stats(self, eventtime): if self.work_timer is None: return False, "" - return True, self.gcode_provider.stats(eventtime) + return self.gcode_provider.get_stats(eventtime) def get_status(self, eventtime): sts = {'is_active': self.is_active()} if self.gcode_provider: From a1cf7e7738ec121f586797908c4c0a9690b13053 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 6 Dec 2022 22:23:31 +0100 Subject: [PATCH 04/35] pa_test: Some improvements for the PA test Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 78 ++++++++++++++++++++++++++-------------- 1 file changed, 52 insertions(+), 26 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index a1e6047cf..3ba7ba69c 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -6,9 +6,12 @@ import logging, math FIRST_LAYER_WIDTH_MULTIPLIER = 1.2 -DEFAULT_X_SIZE = 100.0 -DEFAULT_Y_SIZE = 50.0 +DEFAULT_X_SIZE = 60.0 +DEFAULT_Y_SIZE = 40.0 MAX_YX_SIZE_RATIO = 0.8 +SLOW_NOTCH_SIZE = 2.0 +SEAM_GAP_RATIO = 0.15 +SEAM_EXTRA_WIPE_RATIO = 1.1 class PATest: def __init__(self, config): @@ -32,7 +35,7 @@ def __init__(self, config): above=self.layer_height) self.perimeters = config.getint('perimeters', 2, minval=1) self.brim_width = config.getfloat('brim_width', 10., minval=2.) - self.slow_velocity = config.getfloat('slow_velocity', 20., above=0.) + self.slow_velocity = config.getfloat('slow_velocity', 30., above=0.) self.fast_velocity = config.getfloat('fast_velocity', 80., above=self.slow_velocity) self.filament_diameter = config.getfloat('filament_diameter', 1.75, @@ -111,6 +114,7 @@ def get_gcode(self): 'the expected temperature (e.g. with M109 gcode in a ' 'gcode_macro prior to calling PRINT_PA_TOWER)' % ( heater_status['target'], target_temp)) + toolhead_status = toolhead.get_status(systime) # Get tower params with overrides from the GCode command origin_x = gcmd.get_float('ORIGIN_X', self.origin_x) @@ -123,6 +127,9 @@ def get_gcode(self): above=0.) fast_velocity = gcmd.get_float('FAST_VELOCITY', self.fast_velocity, above=slow_velocity) + scv_velocity = gcmd.get_float('SCV_VELOCITY', + toolhead_status['square_corner_velocity'], + above=0.) filament_diameter = gcmd.get_float('FILAMENT_DIAMETER', self.filament_diameter, above=0.) layer_height = gcmd.get_float('LAYER_HEIGHT', @@ -179,7 +186,7 @@ def gen_brim(): self.progress = start_z / height def gen_tower(): z = first_layer_height + layer_height - x_switching_pos = .25 * size_x + x_switching_pos = size_x / 3. extr_r = 4. * layer_height * nozzle / ( math.pi * filament_diameter**2) while z < height - 0.00000001: @@ -193,33 +200,46 @@ def gen_tower(): yield 'G1 Z%.6f F%.f' % (z, slow_velocity * 60.) for i in range(perimeters): # Print the perimiter loop alternating velocities - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - x_switching_pos, - origin_y + perimeter_y_offset, - x_switching_pos * extr_r, slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y + perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, + perimeter_x_offset * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, origin_y, - perimeter_y_offset * extr_r, - fast_velocity * 60.) + origin_x - perimeter_x_offset, + origin_y + .5 * SLOW_NOTCH_SIZE, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y - .5 * SLOW_NOTCH_SIZE, + SLOW_NOTCH_SIZE * extr_r, + .5 * scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y - perimeter_y_offset, - perimeter_y_offset * extr_r, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - x_switching_pos, origin_y - perimeter_y_offset, (perimeter_x_offset - x_switching_pos) * extr_r, slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - .5 * SLOW_NOTCH_SIZE, + origin_y - perimeter_y_offset, + (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + .5 * SLOW_NOTCH_SIZE, + origin_y - perimeter_y_offset, + SLOW_NOTCH_SIZE * extr_r, scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + x_switching_pos, origin_y - perimeter_y_offset, - 2 * x_switching_pos * extr_r, + (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, @@ -227,35 +247,41 @@ def gen_tower(): (perimeter_x_offset - x_switching_pos) * extr_r, slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, origin_y, - perimeter_y_offset * extr_r, - slow_velocity * 60.) + origin_x + perimeter_x_offset, + origin_y - .5 * SLOW_NOTCH_SIZE, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + perimeter_x_offset, + origin_y + .5 * SLOW_NOTCH_SIZE, + SLOW_NOTCH_SIZE * extr_r, + 2. * scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + perimeter_y_offset, - perimeter_y_offset * extr_r, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + x_switching_pos, + origin_x + nozzle * SEAM_GAP_RATIO, origin_y + perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, + (perimeter_x_offset + - nozzle * SEAM_GAP_RATIO) * extr_r, fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x, origin_y + perimeter_y_offset, - x_switching_pos * extr_r, slow_velocity * 60.) if i < perimeters - 1: # Switch to the next perimeter perimeter_x_offset += nozzle perimeter_y_offset += nozzle yield 'G1 X%.3f Y%.3f F%.f' % ( origin_x, origin_y + perimeter_y_offset, - slow_velocity * 60.) + fast_velocity * 60.) else: # Hide the seam a bit yield 'G1 X%.3f Y%.3f F%.f' % ( - origin_x - 2. * nozzle, + origin_x - nozzle * SEAM_EXTRA_WIPE_RATIO, origin_y + perimeter_y_offset, - slow_velocity * 60.) + fast_velocity * 60.) self.progress = z / height z += layer_height From 3899bae36637176f5039433ae0535c706a154dec Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 12 Feb 2023 00:11:44 +0100 Subject: [PATCH 05/35] pa_test: Changed PA test for square corner velocity testing Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 61 +++++++++++++++++++++++++--------------- 1 file changed, 38 insertions(+), 23 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index 3ba7ba69c..a3ce78869 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -6,12 +6,13 @@ import logging, math FIRST_LAYER_WIDTH_MULTIPLIER = 1.2 -DEFAULT_X_SIZE = 60.0 +DEFAULT_X_SIZE = 50.0 DEFAULT_Y_SIZE = 40.0 MAX_YX_SIZE_RATIO = 0.8 -SLOW_NOTCH_SIZE = 2.0 -SEAM_GAP_RATIO = 0.15 +SLOW_NOTCH_SIZE = 10.0 +SEAM_GAP_RATIO = 0.10 SEAM_EXTRA_WIPE_RATIO = 1.1 +VERY_SLOW_SEG = 0.01 class PATest: def __init__(self, config): @@ -20,9 +21,12 @@ def __init__(self, config): self.size_x = config.getfloat('size_x', 0., minval=0.) self.size_y = config.getfloat('size_y', 0., minval=0.) if self.size_x or self.size_y: - if self.size_x <= 0. or self.size_y <= 0: + if self.size_x < SLOW_NOTCH_SIZE * 4: raise config.error( - 'Both size_x and size_y must be set to positive values') + 'size_x must be at least %.1f' % (SLOW_NOTCH_SIZE * 4,)) + if self.size_y < SLOW_NOTCH_SIZE * 3: + raise config.error( + 'size_y must be at least %.1f' % (SLOW_NOTCH_SIZE * 3,)) max_size_y = MAX_YX_SIZE_RATIO * self.size_x if self.size_y > max_size_y: raise config.error('Too large size_y value, maximum is %.3f' % ( @@ -35,7 +39,7 @@ def __init__(self, config): above=self.layer_height) self.perimeters = config.getint('perimeters', 2, minval=1) self.brim_width = config.getfloat('brim_width', 10., minval=2.) - self.slow_velocity = config.getfloat('slow_velocity', 30., above=0.) + self.slow_velocity = config.getfloat('slow_velocity', 25., above=0.) self.fast_velocity = config.getfloat('fast_velocity', 80., above=self.slow_velocity) self.filament_diameter = config.getfloat('filament_diameter', 1.75, @@ -119,8 +123,8 @@ def get_gcode(self): # Get tower params with overrides from the GCode command origin_x = gcmd.get_float('ORIGIN_X', self.origin_x) origin_y = gcmd.get_float('ORIGIN_Y', self.origin_y) - size_x = gcmd.get_float('SIZE_X', self.size_x) - size_y = gcmd.get_float('SIZE_Y', self.size_y, + size_x = gcmd.get_float('SIZE_X', self.size_x, minval=SLOW_NOTCH_SIZE*4) + size_y = gcmd.get_float('SIZE_Y', self.size_y, minval=SLOW_NOTCH_SIZE*3, maxval=MAX_YX_SIZE_RATIO*size_x) perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) slow_velocity = gcmd.get_float('SLOW_VELOCITY', self.slow_velocity, @@ -210,58 +214,69 @@ def gen_tower(): origin_y + .5 * SLOW_NOTCH_SIZE, (perimeter_y_offset - .5 * SLOW_NOTCH_SIZE) * extr_r, - slow_velocity * 60.) + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y - .5 * SLOW_NOTCH_SIZE, SLOW_NOTCH_SIZE * extr_r, - .5 * scv_velocity * 60.) + slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y - perimeter_y_offset, (perimeter_y_offset - .5 * SLOW_NOTCH_SIZE) * extr_r, - slow_velocity * 60.) + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - x_switching_pos, origin_y - perimeter_y_offset, (perimeter_x_offset - x_switching_pos) * extr_r, - slow_velocity * 60.) + fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - x_switching_pos + VERY_SLOW_SEG, + origin_y - perimeter_y_offset, + VERY_SLOW_SEG * extr_r, + .5 * scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - .5 * SLOW_NOTCH_SIZE, origin_y - perimeter_y_offset, - (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, + (x_switching_pos - .5 * SLOW_NOTCH_SIZE + - VERY_SLOW_SEG) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + .5 * SLOW_NOTCH_SIZE, origin_y - perimeter_y_offset, - SLOW_NOTCH_SIZE * extr_r, scv_velocity * 60.) + SLOW_NOTCH_SIZE * extr_r, + slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + x_switching_pos, origin_y - perimeter_y_offset, (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + x_switching_pos + VERY_SLOW_SEG, + origin_y - perimeter_y_offset, + VERY_SLOW_SEG * extr_r, + scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y - perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, - slow_velocity * 60.) + (perimeter_x_offset - x_switching_pos + - VERY_SLOW_SEG) * extr_r, + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y - .5 * SLOW_NOTCH_SIZE, - (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, + origin_y, + perimeter_y_offset * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y + .5 * SLOW_NOTCH_SIZE, - SLOW_NOTCH_SIZE * extr_r, + origin_y + VERY_SLOW_SEG, + VERY_SLOW_SEG * extr_r, 2. * scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + perimeter_y_offset, - (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, + (perimeter_y_offset - VERY_SLOW_SEG) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + nozzle * SEAM_GAP_RATIO, From 58cccaeb37d196c3b63bad99859fbd96e1e0b244 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 15 Feb 2023 17:13:39 +0100 Subject: [PATCH 06/35] pa_test: Added medium velocity to the test Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 59 ++++++++++++++++++++++++++++++---------- 1 file changed, 44 insertions(+), 15 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index a3ce78869..038a6e776 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -40,8 +40,10 @@ def __init__(self, config): self.perimeters = config.getint('perimeters', 2, minval=1) self.brim_width = config.getfloat('brim_width', 10., minval=2.) self.slow_velocity = config.getfloat('slow_velocity', 25., above=0.) + self.medium_velocity = config.getfloat('medium_velocity', 50., + above=self.slow_velocity) self.fast_velocity = config.getfloat('fast_velocity', 80., - above=self.slow_velocity) + above=self.medium_velocity) self.filament_diameter = config.getfloat('filament_diameter', 1.75, above=0.) # Register commands @@ -129,8 +131,11 @@ def get_gcode(self): perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) slow_velocity = gcmd.get_float('SLOW_VELOCITY', self.slow_velocity, above=0.) + medium_velocity = gcmd.get_float('MEDIUM_VELOCITY', + self.medium_velocity, + above=slow_velocity) fast_velocity = gcmd.get_float('FAST_VELOCITY', self.fast_velocity, - above=slow_velocity) + above=medium_velocity) scv_velocity = gcmd.get_float('SCV_VELOCITY', toolhead_status['square_corner_velocity'], above=0.) @@ -142,6 +147,7 @@ def get_gcode(self): self.first_layer_height, above=layer_height) height = gcmd.get_float('HEIGHT', self.height, above=0.) + step_height = gcmd.get_float('STEP_HEIGHT', 0., minval=0.) brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width, above=nozzle) final_gcode_id = gcmd.get('FINAL_GCODE_ID', None) @@ -189,13 +195,23 @@ def gen_brim(): brim_x_offset * extr_r, slow_velocity * 60.) self.progress = start_z / height def gen_tower(): + last_z = first_layer_height z = first_layer_height + layer_height x_switching_pos = size_x / 3. - extr_r = 4. * layer_height * nozzle / ( - math.pi * filament_diameter**2) while z < height - 0.00000001: + line_width = nozzle perimeter_x_offset = .5 * inner_size_x perimeter_y_offset = .5 * inner_size_y + if (step_height and math.floor(z / step_height) > + math.floor(last_z / step_height)): + # Generate a bit thicker wall for better visual separation + # of bands with different values of Pressure Advance params + line_width += 0.6 * nozzle / perimeters + extra_offs = .5 * (line_width - nozzle) * (perimeters - 1) + perimeter_x_offset -= extra_offs + perimeter_y_offset -= extra_offs + extr_r = 4. * layer_height * line_width / ( + math.pi * filament_diameter**2) # Move to the start of the perimeter yield 'G1 X%.3f Y%.3f F%.f' % ( origin_x, origin_y + perimeter_y_offset, @@ -205,10 +221,15 @@ def gen_tower(): for i in range(perimeters): # Print the perimiter loop alternating velocities yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, + origin_x - x_switching_pos, origin_y + perimeter_y_offset, - perimeter_x_offset * extr_r, + x_switching_pos * extr_r, fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y + .5 * SLOW_NOTCH_SIZE, @@ -265,29 +286,36 @@ def gen_tower(): fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y, - perimeter_y_offset * extr_r, + origin_y - .5 * SLOW_NOTCH_SIZE, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y + VERY_SLOW_SEG, - VERY_SLOW_SEG * extr_r, - 2. * scv_velocity * 60.) + origin_y + .5 * SLOW_NOTCH_SIZE, + SLOW_NOTCH_SIZE * extr_r, + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + perimeter_y_offset, - (perimeter_y_offset - VERY_SLOW_SEG) * extr_r, + (perimeter_y_offset + - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + x_switching_pos, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + nozzle * SEAM_GAP_RATIO, origin_y + perimeter_y_offset, - (perimeter_x_offset + (x_switching_pos - nozzle * SEAM_GAP_RATIO) * extr_r, fast_velocity * 60.) if i < perimeters - 1: # Switch to the next perimeter - perimeter_x_offset += nozzle - perimeter_y_offset += nozzle + perimeter_x_offset += line_width + perimeter_y_offset += line_width yield 'G1 X%.3f Y%.3f F%.f' % ( origin_x, origin_y + perimeter_y_offset, fast_velocity * 60.) @@ -298,6 +326,7 @@ def gen_tower(): origin_y + perimeter_y_offset, fast_velocity * 60.) self.progress = z / height + last_z = z z += layer_height yield 'M83' From 057e15859fbd0da5b908369332f66ac6c9259823 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 19 Feb 2023 14:50:18 +0100 Subject: [PATCH 07/35] pa_test: Replaced one of very slow speeds with dwell Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index 038a6e776..4b12f2660 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -252,16 +252,11 @@ def gen_tower(): origin_y - perimeter_y_offset, (perimeter_x_offset - x_switching_pos) * extr_r, fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - x_switching_pos + VERY_SLOW_SEG, - origin_y - perimeter_y_offset, - VERY_SLOW_SEG * extr_r, - .5 * scv_velocity * 60.) + yield 'G4 P0.001' yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - .5 * SLOW_NOTCH_SIZE, origin_y - perimeter_y_offset, - (x_switching_pos - .5 * SLOW_NOTCH_SIZE - - VERY_SLOW_SEG) * extr_r, + (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + .5 * SLOW_NOTCH_SIZE, From 0b801349a8128b1b5386dbcd3cd58326aaa86a65 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Fri, 7 Apr 2023 14:42:20 +0200 Subject: [PATCH 08/35] pa_test: Added one dwell to the PA test on Y axis Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index 4b12f2660..fd156317a 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -198,6 +198,7 @@ def gen_tower(): last_z = first_layer_height z = first_layer_height + layer_height x_switching_pos = size_x / 3. + fast_notch_size = size_y - 2. * SLOW_NOTCH_SIZE while z < height - 0.00000001: line_width = nozzle perimeter_x_offset = .5 * inner_size_x @@ -281,21 +282,27 @@ def gen_tower(): fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y - .5 * SLOW_NOTCH_SIZE, + origin_y - .5 * fast_notch_size, (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, + - .5 * fast_notch_size) * extr_r, + medium_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x + perimeter_x_offset, + origin_y, + .5 * fast_notch_size * extr_r, fast_velocity * 60.) + yield 'G4 P0.001' yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, - origin_y + .5 * SLOW_NOTCH_SIZE, - SLOW_NOTCH_SIZE * extr_r, - medium_velocity * 60.) + origin_y + .5 * fast_notch_size, + .5 * fast_notch_size * extr_r, + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + perimeter_y_offset, (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, - fast_velocity * 60.) + - .5 * fast_notch_size) * extr_r, + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + x_switching_pos, origin_y + perimeter_y_offset, From 58afffc5637640c3968b241c574b7ffad916e3b0 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 2 Apr 2023 16:17:48 +0200 Subject: [PATCH 09/35] extruder: Split extruder motion into X/Y/Z components This should simplify subsequent input shaping of extruder. Signed-off-by: Dmitry Butyugin --- klippy/chelper/kin_extruder.c | 52 ++++++++++++++++++++------------- klippy/kinematics/extruder.py | 55 +++++++++++++++-------------------- 2 files changed, 56 insertions(+), 51 deletions(-) diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index b8d1cc221..960570a4e 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -52,7 +52,7 @@ extruder_integrate_time(double base, double start_v, double half_accel // Calculate the definitive integral of extruder for a given move static double -pa_move_integrate(struct move *m, double pressure_advance +pa_move_integrate(struct move *m, int axis, double pressure_advance , double base, double start, double end, double time_offset) { if (start < 0.) @@ -60,13 +60,15 @@ pa_move_integrate(struct move *m, double pressure_advance if (end > m->move_t) end = m->move_t; // Calculate base position and velocity with pressure advance - int can_pressure_advance = m->axes_r.y != 0.; + int can_pressure_advance = m->axes_r.x > 0. || m->axes_r.y > 0.; if (!can_pressure_advance) pressure_advance = 0.; - base += pressure_advance * m->start_v; - double start_v = m->start_v + pressure_advance * 2. * m->half_accel; + double axis_r = m->axes_r.axis[axis - 'x']; + double start_v = m->start_v * axis_r; + double ha = m->half_accel * axis_r; + base += pressure_advance * start_v; + start_v += pressure_advance * 2. * ha; // Calculate definitive integral - double ha = m->half_accel; double iext = extruder_integrate(base, start_v, ha, start, end); double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end); return wgt_ext - time_offset * iext; @@ -74,29 +76,31 @@ pa_move_integrate(struct move *m, double pressure_advance // Calculate the definitive integral of the extruder over a range of moves static double -pa_range_integrate(struct move *m, double move_time +pa_range_integrate(struct move *m, int axis, double move_time , double pressure_advance, double hst) { // Calculate integral for the current move double res = 0., start = move_time - hst, end = move_time + hst; - double start_base = m->start_pos.x; - res += pa_move_integrate(m, pressure_advance, 0., start, move_time, start); - res -= pa_move_integrate(m, pressure_advance, 0., move_time, end, end); + double start_base = m->start_pos.axis[axis - 'x']; + res += pa_move_integrate(m, axis, pressure_advance, 0. + , start, move_time, start); + res -= pa_move_integrate(m, axis, pressure_advance, 0. + , move_time, end, end); // Integrate over previous moves struct move *prev = m; while (unlikely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; - double base = prev->start_pos.x - start_base; - res += pa_move_integrate(prev, pressure_advance, base, start + double base = prev->start_pos.axis[axis - 'x'] - start_base; + res += pa_move_integrate(prev, axis, pressure_advance, base, start , prev->move_t, start); } // Integrate over future moves while (unlikely(end > m->move_t)) { end -= m->move_t; m = list_next_entry(m, node); - double base = m->start_pos.x - start_base; - res -= pa_move_integrate(m, pressure_advance, base, 0., end, end); + double base = m->start_pos.axis[axis - 'x'] - start_base; + res -= pa_move_integrate(m, axis, pressure_advance, base, 0., end, end); } return res; } @@ -112,12 +116,20 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); double hst = es->half_smooth_time; - if (!hst) - // Pressure advance not enabled - return m->start_pos.x + move_get_distance(m, move_time); - // Apply pressure advance and average over smooth_time - double area = pa_range_integrate(m, move_time, es->pressure_advance, hst); - return m->start_pos.x + area * es->inv_half_smooth_time2; + int i; + struct coord e_pos; + double move_dist = move_get_distance(m, move_time); + for (i = 0; i < 3; ++i) { + if (!hst) { + e_pos.axis[i] = m->axes_r.axis[i] * move_dist; + } else { + double area = pa_range_integrate(m, 'x' + i, move_time, + es->pressure_advance, hst); + e_pos.axis[i] = area * es->inv_half_smooth_time2; + } + e_pos.axis[i] += m->start_pos.axis[i]; + } + return e_pos.x + e_pos.y + e_pos.z; } void __visible @@ -140,6 +152,6 @@ extruder_stepper_alloc(void) struct extruder_stepper *es = malloc(sizeof(*es)); memset(es, 0, sizeof(*es)); es->sk.calc_position_cb = extruder_calc_position; - es->sk.active_flags = AF_X; + es->sk.active_flags = AF_X | AF_Y | AF_Z; return &es->sk; } diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 35a2a8723..b8756c04a 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -98,10 +98,9 @@ def sync_to_extruder(self, extruder_name): return extruder = self.printer.lookup_object(extruder_name, None) if extruder is None or not isinstance(extruder, PrinterExtruder): - raise self.printer.command_error( - "'%s' is not a valid extruder." % (extruder_name,) - ) - self.stepper.set_position([extruder.last_position, 0.0, 0.0]) + raise self.printer.command_error("'%s' is not a valid extruder." + % (extruder_name,)) + self.stepper.set_position(extruder.last_position) self.stepper.set_trapq(extruder.get_trapq()) self.motion_queue = extruder_name @@ -217,7 +216,7 @@ class PrinterExtruder: def __init__(self, config, extruder_num): self.printer = config.get_printer() self.name = config.get_name() - self.last_position = 0.0 + self.last_position = [0., 0., 0.] # Setup hotend heater shared_heater = config.get("shared_heater", None) pheaters = self.printer.load_object(config, "heaters") @@ -354,31 +353,25 @@ def calc_junction(self, prev_move, move): def move(self, print_time, move): axis_r = move.axes_r[3] - accel = move.accel * axis_r - start_v = move.start_v * axis_r - cruise_v = move.cruise_v * axis_r - can_pressure_advance = False - if axis_r > 0.0 and (move.axes_d[0] or move.axes_d[1]): - can_pressure_advance = True - # Queue movement (x is extruder movement, y is pressure advance flag) - self.trapq_append( - self.trapq, - print_time, - move.accel_t, - move.cruise_t, - move.decel_t, - move.start_pos[3], - 0.0, - 0.0, - 1.0, - can_pressure_advance, - 0.0, - start_v, - cruise_v, - accel, - ) - self.last_position = move.end_pos[3] - + abs_axis_r = abs(axis_r) + accel = move.accel * abs_axis_r + start_v = move.start_v * abs_axis_r + cruise_v = move.cruise_v * abs_axis_r + extr_pos = self.last_position + if move.is_kinematic_move: + # Regular kinematic move with extrusion + extr_r = [math.copysign(r * r, axis_r) for r in move.axes_r[:3]] + else: + # Extrude-only move, do not apply pressure advance + extr_r = [0., 0., axis_r] + self.trapq_append(self.trapq, print_time, + move.accel_t, move.cruise_t, move.decel_t, + extr_pos[0], extr_pos[1], extr_pos[2], + extr_r[0], extr_r[1], extr_r[2], + start_v, cruise_v, accel) + extr_d = abs(move.axes_d[3]) + for i in range(3): + self.last_position[i] += extr_d * extr_r[i] def find_past_position(self, print_time): if self.extruder_stepper is None: return 0.0 @@ -415,7 +408,7 @@ def cmd_ACTIVATE_EXTRUDER(self, gcmd): return gcmd.respond_info("Activating extruder %s" % (self.name,)) toolhead.flush_step_generation() - toolhead.set_extruder(self, self.last_position) + toolhead.set_extruder(self, sum(self.last_position)) self.printer.send_event("extruder:activate_extruder") From eca1caa2eec80c27586491b60fe662f6b7bb2879 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 12 Feb 2023 00:45:05 +0100 Subject: [PATCH 10/35] extruder: Added support for time offset of extruder vs kinematic moves Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 2 +- klippy/chelper/kin_extruder.c | 30 ++++++++++++++-- klippy/kinematics/extruder.py | 67 +++++++++++++++++------------------ 3 files changed, 61 insertions(+), 38 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 2576105d5..011ad5dbd 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -161,7 +161,7 @@ defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); void extruder_set_pressure_advance(struct stepper_kinematics *sk - , double pressure_advance, double smooth_time); + , double pressure_advance, double smooth_time, double time_offset); """ defs_kin_shaper = """ diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 960570a4e..712fd5557 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -107,7 +107,8 @@ pa_range_integrate(struct move *m, int axis, double move_time struct extruder_stepper { struct stepper_kinematics sk; - double pressure_advance, half_smooth_time, inv_half_smooth_time2; + double pressure_advance, time_offset; + double half_smooth_time, inv_half_smooth_time2; }; static double @@ -115,6 +116,15 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + move_time += es->time_offset; + while (unlikely(move_time < 0.)) { + m = list_prev_entry(m, node); + move_time += m->move_t; + } + while (unlikely(move_time >= m->move_t)) { + move_time -= m->move_t; + m = list_next_entry(m, node); + } double hst = es->half_smooth_time; int i; struct coord e_pos; @@ -132,14 +142,28 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m return e_pos.x + e_pos.y + e_pos.z; } +static void +extruder_note_generation_time(struct extruder_stepper *es) +{ + double pre_active = 0., post_active = 0.; + pre_active += es->half_smooth_time + es->time_offset; + if (pre_active < 0.) pre_active = 0.; + post_active += es->half_smooth_time - es->time_offset; + if (post_active < 0.) post_active = 0.; + es->sk.gen_steps_pre_active = pre_active; + es->sk.gen_steps_post_active = post_active; +} + void __visible extruder_set_pressure_advance(struct stepper_kinematics *sk - , double pressure_advance, double smooth_time) + , double pressure_advance, double smooth_time + , double time_offset) { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); double hst = smooth_time * .5; es->half_smooth_time = hst; - es->sk.gen_steps_pre_active = es->sk.gen_steps_post_active = hst; + es->time_offset = time_offset; + extruder_note_generation_time(es); if (! hst) return; es->inv_half_smooth_time2 = 1. / (hst * hst); diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index b8756c04a..4e2821357 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -11,11 +11,13 @@ class ExtruderStepper: def __init__(self, config): self.printer = config.get_printer() self.name = config.get_name().split()[-1] - self.pressure_advance = self.pressure_advance_smooth_time = 0.0 - self.config_pa = config.getfloat("pressure_advance", 0.0, minval=0.0) + self.pressure_advance = self.pressure_advance_smooth_time = 0. + self.pressure_advance_time_offset = 0. + self.config_pa = config.getfloat('pressure_advance', 0., minval=0.) self.config_smooth_time = config.getfloat( - "pressure_advance_smooth_time", 0.040, above=0.0, maxval=0.200 - ) + 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) + self.config_time_offset = config.getfloat( + 'pressure_advance_time_offset', 0.0, minval=-0.2, maxval=0.2) # Setup stepper self.stepper = stepper.PrinterStepper(config) ffi_main, ffi_lib = chelper.get_ffi() @@ -76,15 +78,13 @@ def __init__(self, config): def _handle_connect(self): toolhead = self.printer.lookup_object("toolhead") toolhead.register_step_generator(self.stepper.generate_steps) - self._set_pressure_advance(self.config_pa, self.config_smooth_time) - + self._set_pressure_advance(self.config_pa, self.config_smooth_time, + self.config_time_offset) def get_status(self, eventtime): - return { - "pressure_advance": self.pressure_advance, - "smooth_time": self.pressure_advance_smooth_time, - "motion_queue": self.motion_queue, - } - + return {'pressure_advance': self.pressure_advance, + 'smooth_time': self.pressure_advance_smooth_time, + 'time_offset': self.pressure_advance_time_offset, + 'motion_queue': self.motion_queue} def find_past_position(self, print_time): mcu_pos = self.stepper.get_past_mcu_position(print_time) return self.stepper.mcu_to_commanded_position(mcu_pos) @@ -103,24 +103,25 @@ def sync_to_extruder(self, extruder_name): self.stepper.set_position(extruder.last_position) self.stepper.set_trapq(extruder.get_trapq()) self.motion_queue = extruder_name - - def _set_pressure_advance(self, pressure_advance, smooth_time): + def _set_pressure_advance(self, pressure_advance, + smooth_time, time_offset): old_smooth_time = self.pressure_advance_smooth_time if not self.pressure_advance: - old_smooth_time = 0.0 + old_smooth_time = 0. + old_time_offset = self.pressure_advance_time_offset new_smooth_time = smooth_time if not pressure_advance: new_smooth_time = 0.0 toolhead = self.printer.lookup_object("toolhead") toolhead.note_step_generation_scan_time( - new_smooth_time * 0.5, old_delay=old_smooth_time * 0.5 - ) + new_smooth_time * .5 + abs(time_offset), + old_delay=(old_smooth_time * .5 + abs(old_time_offset))) ffi_main, ffi_lib = chelper.get_ffi() espa = ffi_lib.extruder_set_pressure_advance - espa(self.sk_extruder, pressure_advance, new_smooth_time) + espa(self.sk_extruder, pressure_advance, new_smooth_time, time_offset) self.pressure_advance = pressure_advance self.pressure_advance_smooth_time = smooth_time - + self.pressure_advance_time_offset = time_offset cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): @@ -133,21 +134,19 @@ def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): extruder.extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) def cmd_SET_PRESSURE_ADVANCE(self, gcmd): - pressure_advance = gcmd.get_float( - "ADVANCE", self.pressure_advance, minval=0.0 - ) - smooth_time = gcmd.get_float( - "SMOOTH_TIME", - self.pressure_advance_smooth_time, - minval=0.0, - maxval=0.200, - ) - self._set_pressure_advance(pressure_advance, smooth_time) - msg = ( - "pressure_advance: %.6f\n" - "pressure_advance_smooth_time: %.6f" - % (pressure_advance, smooth_time) - ) + pressure_advance = gcmd.get_float('ADVANCE', self.pressure_advance, + minval=0.) + smooth_time = gcmd.get_float('SMOOTH_TIME', + self.pressure_advance_smooth_time, + minval=0., maxval=.200) + time_offset = gcmd.get_float('TIME_OFFSET', + self.pressure_advance_time_offset, + minval=-0.2, maxval=0.2) + self._set_pressure_advance(pressure_advance, smooth_time, time_offset) + msg = ("pressure_advance: %.6f\n" + "pressure_advance_smooth_time: %.6f\n" + "pressure_advance_time_offset: %.6f" + % (pressure_advance, smooth_time, time_offset)) self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg)) gcmd.respond_info(msg, log=False) From 75313c34f4d5e73ec3ef2a5653e46febfdd71547 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 10 Jan 2023 20:35:11 +0100 Subject: [PATCH 11/35] extruder: Explicit PA velocity term calculation Signed-off-by: Dmitry Butyugin --- klippy/chelper/kin_extruder.c | 75 +++++++++++++++++++++++------------ 1 file changed, 49 insertions(+), 26 deletions(-) diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 712fd5557..b6a955e9f 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -51,9 +51,10 @@ extruder_integrate_time(double base, double start_v, double half_accel } // Calculate the definitive integral of extruder for a given move -static double -pa_move_integrate(struct move *m, int axis, double pressure_advance - , double base, double start, double end, double time_offset) +static void +pa_move_integrate(struct move *m, int axis + , double base, double start, double end, double time_offset + , double *pos_integral, double *pa_velocity_integral) { if (start < 0.) start = 0.; @@ -61,48 +62,61 @@ pa_move_integrate(struct move *m, int axis, double pressure_advance end = m->move_t; // Calculate base position and velocity with pressure advance int can_pressure_advance = m->axes_r.x > 0. || m->axes_r.y > 0.; - if (!can_pressure_advance) - pressure_advance = 0.; double axis_r = m->axes_r.axis[axis - 'x']; double start_v = m->start_v * axis_r; double ha = m->half_accel * axis_r; - base += pressure_advance * start_v; - start_v += pressure_advance * 2. * ha; // Calculate definitive integral double iext = extruder_integrate(base, start_v, ha, start, end); double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end); - return wgt_ext - time_offset * iext; + *pos_integral = wgt_ext - time_offset * iext; + if (!can_pressure_advance) { + *pa_velocity_integral = 0.; + } else { + double ivel = extruder_integrate(start_v, 2. * ha, 0., start, end); + double wgt_vel = extruder_integrate(0., start_v, 2. * ha, start, end); + *pa_velocity_integral = wgt_vel - time_offset * ivel; + } } // Calculate the definitive integral of the extruder over a range of moves -static double -pa_range_integrate(struct move *m, int axis, double move_time - , double pressure_advance, double hst) +static void +pa_range_integrate(struct move *m, int axis, double move_time, double hst + , double *pos_integral, double *pa_velocity_integral) { // Calculate integral for the current move - double res = 0., start = move_time - hst, end = move_time + hst; + *pos_integral = *pa_velocity_integral = 0.; + double m_pos_int, m_pa_vel_int; + double start = move_time - hst, end = move_time + hst; double start_base = m->start_pos.axis[axis - 'x']; - res += pa_move_integrate(m, axis, pressure_advance, 0. - , start, move_time, start); - res -= pa_move_integrate(m, axis, pressure_advance, 0. - , move_time, end, end); + pa_move_integrate(m, axis, 0., start, move_time, start, + &m_pos_int, &m_pa_vel_int); + *pos_integral += m_pos_int; + *pa_velocity_integral += m_pa_vel_int; + pa_move_integrate(m, axis, 0., move_time, end, end, + &m_pos_int, &m_pa_vel_int); + *pos_integral -= m_pos_int; + *pa_velocity_integral -= m_pa_vel_int; // Integrate over previous moves struct move *prev = m; while (unlikely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; double base = prev->start_pos.axis[axis - 'x'] - start_base; - res += pa_move_integrate(prev, axis, pressure_advance, base, start - , prev->move_t, start); + pa_move_integrate(prev, axis, base, start, prev->move_t, start, + &m_pos_int, &m_pa_vel_int); + *pos_integral += m_pos_int; + *pa_velocity_integral += m_pa_vel_int; } // Integrate over future moves while (unlikely(end > m->move_t)) { end -= m->move_t; m = list_next_entry(m, node); double base = m->start_pos.axis[axis - 'x'] - start_base; - res -= pa_move_integrate(m, axis, pressure_advance, base, 0., end, end); + pa_move_integrate(m, axis, base, 0., end, end, + &m_pos_int, &m_pa_vel_int); + *pos_integral -= m_pos_int; + *pa_velocity_integral -= m_pa_vel_int; } - return res; } struct extruder_stepper { @@ -127,19 +141,26 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m } double hst = es->half_smooth_time; int i; - struct coord e_pos; + struct coord e_pos, pa_vel; double move_dist = move_get_distance(m, move_time); for (i = 0; i < 3; ++i) { if (!hst) { e_pos.axis[i] = m->axes_r.axis[i] * move_dist; + pa_vel.axis[i] = 0.; } else { - double area = pa_range_integrate(m, 'x' + i, move_time, - es->pressure_advance, hst); - e_pos.axis[i] = area * es->inv_half_smooth_time2; + pa_range_integrate(m, 'x' + i, move_time, hst, + &e_pos.axis[i], &pa_vel.axis[i]); + e_pos.axis[i] *= es->inv_half_smooth_time2; + pa_vel.axis[i] *= es->inv_half_smooth_time2; } e_pos.axis[i] += m->start_pos.axis[i]; } - return e_pos.x + e_pos.y + e_pos.z; + double position = e_pos.x + e_pos.y + e_pos.z; + double pa_velocity = pa_vel.x + pa_vel.y + pa_vel.z; + if (hst) { + position += es->pressure_advance * pa_velocity; + } + return position; } static void @@ -164,8 +185,10 @@ extruder_set_pressure_advance(struct stepper_kinematics *sk es->half_smooth_time = hst; es->time_offset = time_offset; extruder_note_generation_time(es); - if (! hst) + if (! hst) { + es->pressure_advance = 0.; return; + } es->inv_half_smooth_time2 = 1. / (hst * hst); es->pressure_advance = pressure_advance; } From 51ed06e30bd1ede5f1d7c467fb9202da40a8655b Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 15 Feb 2023 17:07:39 +0100 Subject: [PATCH 12/35] extruder: Improve numerical stability of time-weighted averaging Signed-off-by: Dmitry Butyugin --- klippy/chelper/kin_extruder.c | 44 ++++++++++++++--------------------- 1 file changed, 18 insertions(+), 26 deletions(-) diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index b6a955e9f..af3295043 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -25,29 +25,23 @@ // from=t-smooth_time/2, to=t+smooth_time/2) // / ((smooth_time/2)**2)) -// Calculate the definitive integral of the motion formula: -// position(t) = base + t * (start_v + t * half_accel) -static double -extruder_integrate(double base, double start_v, double half_accel - , double start, double end) -{ - double half_v = .5 * start_v, sixth_a = (1. / 3.) * half_accel; - double si = start * (base + start * (half_v + start * sixth_a)); - double ei = end * (base + end * (half_v + end * sixth_a)); - return ei - si; -} - // Calculate the definitive integral of time weighted position: // weighted_position(t) = t * (base + t * (start_v + t * half_accel)) -static double -extruder_integrate_time(double base, double start_v, double half_accel - , double start, double end) +// - time_offset * (base + t * (start_v + t * half_accel)) +inline static double +extruder_integrate_weighted(double base, double start_v, double half_accel + , double start, double end, double time_offset) { - double half_b = .5 * base, third_v = (1. / 3.) * start_v; - double eighth_a = .25 * half_accel; - double si = start * start * (half_b + start * (third_v + start * eighth_a)); - double ei = end * end * (half_b + end * (third_v + end * eighth_a)); - return ei - si; + double delta_t = end - start; + double end2 = end * end; + double start2 = start * start; + double c1 = .5 * (end + start); + double c2 = 1./3. * (end2 + end * start + start2); + double c3 = .5 * c1 * (end2 + start2); + double avg_val = base * (c1 - time_offset) + + start_v * (c2 - c1 * time_offset) + + half_accel * (c3 - c2 * time_offset); + return delta_t * avg_val; } // Calculate the definitive integral of extruder for a given move @@ -66,15 +60,13 @@ pa_move_integrate(struct move *m, int axis double start_v = m->start_v * axis_r; double ha = m->half_accel * axis_r; // Calculate definitive integral - double iext = extruder_integrate(base, start_v, ha, start, end); - double wgt_ext = extruder_integrate_time(base, start_v, ha, start, end); - *pos_integral = wgt_ext - time_offset * iext; + *pos_integral = extruder_integrate_weighted( + base, start_v, ha, start, end, time_offset); if (!can_pressure_advance) { *pa_velocity_integral = 0.; } else { - double ivel = extruder_integrate(start_v, 2. * ha, 0., start, end); - double wgt_vel = extruder_integrate(0., start_v, 2. * ha, start, end); - *pa_velocity_integral = wgt_vel - time_offset * ivel; + *pa_velocity_integral = extruder_integrate_weighted( + start_v, 2. * ha, 0., start, end, time_offset); } } From dabfd2be78071c8aee4d433c390b7608af9f7d96 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 10 Jan 2023 22:23:03 +0100 Subject: [PATCH 13/35] extruder: Added support for non-linear Pressure Advance Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 12 ++- klippy/chelper/kin_extruder.c | 83 +++++++++++++++--- klippy/kinematics/extruder.py | 161 +++++++++++++++++++++++++++++----- 3 files changed, 220 insertions(+), 36 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 011ad5dbd..8bbd27068 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -161,7 +161,17 @@ defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); void extruder_set_pressure_advance(struct stepper_kinematics *sk - , double pressure_advance, double smooth_time, double time_offset); + , int n_params, double params[] + , double smooth_time, double time_offset); + struct pressure_advance_params; + double pressure_advance_linear_model_func(double position + , double pa_velocity, struct pressure_advance_params *pa_params); + double pressure_advance_tanh_model_func(double position + , double pa_velocity, struct pressure_advance_params *pa_params); + double pressure_advance_recipr_model_func(double position + , double pa_velocity, struct pressure_advance_params *pa_params); + void extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk + , double (*func)(double, double, struct pressure_advance_params *)); """ defs_kin_shaper = """ diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index af3295043..77a1c01b2 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -4,6 +4,7 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. +#include // tanh #include // offsetof #include // malloc #include // memset @@ -111,12 +112,59 @@ pa_range_integrate(struct move *m, int axis, double move_time, double hst } } +struct pressure_advance_params { + union { + struct { + double pressure_advance; + }; + struct { + double linear_advance, linear_offset, linearization_velocity; + }; + double params[3]; + }; +}; + +typedef double (*pressure_advance_func)( + double, double, struct pressure_advance_params *pa_params); + struct extruder_stepper { struct stepper_kinematics sk; - double pressure_advance, time_offset; - double half_smooth_time, inv_half_smooth_time2; + struct pressure_advance_params pa_params; + pressure_advance_func pa_func; + double time_offset, half_smooth_time, inv_half_smooth_time2; }; +double __visible +pressure_advance_linear_model_func(double position, double pa_velocity + , struct pressure_advance_params *pa_params) +{ + return position + pa_velocity * pa_params->pressure_advance; +} + +double __visible +pressure_advance_tanh_model_func(double position, double pa_velocity + , struct pressure_advance_params *pa_params) +{ + position += pa_params->linear_advance * pa_velocity; + if (pa_params->linear_offset) { + double rel_velocity = pa_velocity / pa_params->linearization_velocity; + position += pa_params->linear_offset * tanh(rel_velocity); + } + return position; +} + +double __visible +pressure_advance_recipr_model_func(double position, double pa_velocity + , struct pressure_advance_params *pa_params) +{ + position += pa_params->linear_advance * pa_velocity; + if (pa_params->linear_offset) { + double rel_velocity = pa_velocity / pa_params->linearization_velocity; + position += pa_params->linear_offset * (1. - 1. / (1. + rel_velocity)); + } + return position; +} + static double extruder_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) @@ -148,11 +196,11 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m e_pos.axis[i] += m->start_pos.axis[i]; } double position = e_pos.x + e_pos.y + e_pos.z; + if (!hst) + return position; double pa_velocity = pa_vel.x + pa_vel.y + pa_vel.z; - if (hst) { - position += es->pressure_advance * pa_velocity; - } - return position; + if (pa_velocity < 0.) pa_velocity = 0.; + return es->pa_func(position, pa_velocity, &es->pa_params); } static void @@ -169,20 +217,30 @@ extruder_note_generation_time(struct extruder_stepper *es) void __visible extruder_set_pressure_advance(struct stepper_kinematics *sk - , double pressure_advance, double smooth_time - , double time_offset) + , int n_params, double params[] + , double smooth_time, double time_offset) { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); double hst = smooth_time * .5; es->half_smooth_time = hst; es->time_offset = time_offset; extruder_note_generation_time(es); - if (! hst) { - es->pressure_advance = 0.; + if (! hst) return; - } es->inv_half_smooth_time2 = 1. / (hst * hst); - es->pressure_advance = pressure_advance; + memset(&es->pa_params, 0, sizeof(es->pa_params)); + if (n_params < 0 || n_params > ARRAY_SIZE(es->pa_params.params)) + return; + memcpy(&es->pa_params, params, n_params * sizeof(params[0])); +} + +void __visible +extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk + , pressure_advance_func func) +{ + struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + memset(&es->pa_params, 0, sizeof(es->pa_params)); + es->pa_func = func; } struct stepper_kinematics * __visible @@ -191,6 +249,7 @@ extruder_stepper_alloc(void) struct extruder_stepper *es = malloc(sizeof(*es)); memset(es, 0, sizeof(*es)); es->sk.calc_position_cb = extruder_calc_position; + es->pa_func = pressure_advance_tanh_model_func; es->sk.active_flags = AF_X | AF_Y | AF_Z; return &es->sk; } diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 4e2821357..3f174fc9c 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -6,14 +6,113 @@ import math, logging import stepper, chelper +class PALinearModel: + name = 'linear' + def __init__(self, config=None, other=None): + self.pressure_advance = 0. + if config: + self.pressure_advance = config.getfloat( + 'pressure_advance', self.pressure_advance, minval=0.) + elif other: + self.pressure_advance = other.pressure_advance + def get_from_gcmd(self, gcmd): + new_model = PALinearModel(config=None, other=self) + new_model.pressure_advance = gcmd.get_float( + 'ADVANCE', self.pressure_advance, minval=0.) + return new_model + def enabled(self): + return self.pressure_advance > 0. + def get_pa_params(self): + return (self.pressure_advance,) + def get_status(self, eventtime): + return {'pressure_advance': self.pressure_advance} + def get_msg(self): + return 'pressure_advance: %.6f' % (self.pressure_advance,) + def get_func(self): + ffi_main, ffi_lib = chelper.get_ffi() + return ffi_lib.pressure_advance_linear_model_func + +class PANonLinearModel: + def __init__(self, config=None, other=None): + self.linear_advance = 0. + self.linear_offset = 0. + self.linearization_velocity = 0. + if config: + self.linear_advance = config.getfloat('linear_advance', + 0., minval=0.) + self.linear_offset = config.getfloat('linear_offset', + 0., minval=0.) + if self.linear_offset: + self.linearization_velocity = config.getfloat( + 'linearization_velocity', above=0.) + else: + self.linearization_velocity = config.getfloat( + 'linearization_velocity', 0., minval=0.) + elif other: + self.linear_advance = other.linear_advance + self.linear_offset = other.linear_offset + self.linearization_velocity = other.linearization_velocity + def get_from_gcmd(self, gcmd): + new_model = self.__class__(config=None, other=self) + new_model.linear_advance = gcmd.get_float( + 'ADVANCE', self.linear_advance, minval=0.) + new_model.linear_offset = gcmd.get_float( + 'OFFSET', self.linear_offset, minval=0.) + new_model.linearization_velocity = gcmd.get_float( + 'VELOCITY', self.linearization_velocity) + if new_model.linear_offset and new_model.linearization_velocity <= 0.: + raise gcmd.error('VELOCITY must be set to a positive value ' + 'when OFFSET is non-zero') + return new_model + def enabled(self): + return self.linear_advance > 0. or self.linear_offset > 0. + def get_pa_params(self): + # The order must match the order of parameters in the + # pressure_advance_params struct in kin_extruder.c + return (self.linear_advance, self.linear_offset, + self.linearization_velocity) + def get_status(self, eventtime): + return {'linear_advance': self.linear_advance, + 'linear_offset': self.linear_offset, + 'linearization_velocity': self.linearization_velocity} + def get_msg(self): + return ('linear_advance: %.6f\n' + 'linear_offset: %.6f\n' + 'linearization_velocity: %.6f' % ( + self.linear_advance, + self.linear_offset, + self.linearization_velocity)) + def get_func(self): + return None + +class PATanhModel(PANonLinearModel): + name = 'tanh' + def __init__(self, config=None, other=None): + PANonLinearModel.__init__(self, config, other) + def get_func(self): + ffi_main, ffi_lib = chelper.get_ffi() + return ffi_lib.pressure_advance_tanh_model_func + +class PAReciprModel(PANonLinearModel): + name = 'recipr' + def __init__(self, config=None, other=None): + PANonLinearModel.__init__(self, config, other) + def get_func(self): + ffi_main, ffi_lib = chelper.get_ffi() + return ffi_lib.pressure_advance_recipr_model_func class ExtruderStepper: + pa_models = {PALinearModel.name: PALinearModel, + PATanhModel.name: PATanhModel, + PAReciprModel.name: PAReciprModel} def __init__(self, config): self.printer = config.get_printer() self.name = config.get_name().split()[-1] - self.pressure_advance = self.pressure_advance_smooth_time = 0. + self.pa_model = config.getchoice('pressure_advance_model', + self.pa_models, + PALinearModel.name)(config) + self.pressure_advance_smooth_time = 0. self.pressure_advance_time_offset = 0. - self.config_pa = config.getfloat('pressure_advance', 0., minval=0.) self.config_smooth_time = config.getfloat( 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) self.config_time_offset = config.getfloat( @@ -25,6 +124,8 @@ def __init__(self, config): ffi_lib.extruder_stepper_alloc(), ffi_lib.free ) self.stepper.set_stepper_kinematics(self.sk_extruder) + ffi_lib.extruder_set_pressure_advance_model_func( + self.sk_extruder, self.pa_model.get_func()) self.motion_queue = None # Register commands self.printer.register_event_handler( @@ -78,13 +179,16 @@ def __init__(self, config): def _handle_connect(self): toolhead = self.printer.lookup_object("toolhead") toolhead.register_step_generator(self.stepper.generate_steps) - self._set_pressure_advance(self.config_pa, self.config_smooth_time, - self.config_time_offset) + self._update_pressure_advance(self.pa_model, + self.config_smooth_time, + self.config_time_offset) def get_status(self, eventtime): - return {'pressure_advance': self.pressure_advance, - 'smooth_time': self.pressure_advance_smooth_time, - 'time_offset': self.pressure_advance_time_offset, - 'motion_queue': self.motion_queue} + sts = {'pressure_advance_model': self.pa_model.name, + 'smooth_time': self.pressure_advance_smooth_time, + 'time_offset': self.pressure_advance_time_offset, + 'motion_queue': self.motion_queue} + sts.update(self.pa_model.get_status(eventtime)) + return sts def find_past_position(self, print_time): mcu_pos = self.stepper.get_past_mcu_position(print_time) return self.stepper.mcu_to_commanded_position(mcu_pos) @@ -103,23 +207,28 @@ def sync_to_extruder(self, extruder_name): self.stepper.set_position(extruder.last_position) self.stepper.set_trapq(extruder.get_trapq()) self.motion_queue = extruder_name - def _set_pressure_advance(self, pressure_advance, - smooth_time, time_offset): + def _update_pressure_advance(self, pa_model, smooth_time, time_offset): old_smooth_time = self.pressure_advance_smooth_time - if not self.pressure_advance: + if not self.pa_model.enabled(): old_smooth_time = 0. old_time_offset = self.pressure_advance_time_offset new_smooth_time = smooth_time - if not pressure_advance: - new_smooth_time = 0.0 + if not pa_model.enabled(): + new_smooth_time = 0. toolhead = self.printer.lookup_object("toolhead") toolhead.note_step_generation_scan_time( new_smooth_time * .5 + abs(time_offset), old_delay=(old_smooth_time * .5 + abs(old_time_offset))) ffi_main, ffi_lib = chelper.get_ffi() - espa = ffi_lib.extruder_set_pressure_advance - espa(self.sk_extruder, pressure_advance, new_smooth_time, time_offset) - self.pressure_advance = pressure_advance + if self.pa_model.name != pa_model.name: + pa_func = pa_model.get_func() + ffi_lib.extruder_set_pressure_advance_model_func(self.sk_extruder, + pa_func) + pa_params = pa_model.get_pa_params() + ffi_lib.extruder_set_pressure_advance( + self.sk_extruder, len(pa_params), pa_params, + new_smooth_time, time_offset) + self.pa_model = pa_model self.pressure_advance_smooth_time = smooth_time self.pressure_advance_time_offset = time_offset cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" @@ -134,19 +243,25 @@ def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): extruder.extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) def cmd_SET_PRESSURE_ADVANCE(self, gcmd): - pressure_advance = gcmd.get_float('ADVANCE', self.pressure_advance, - minval=0.) + pa_model_name = gcmd.get('MODEL', self.pa_model.name) + if pa_model_name not in self.pa_models: + raise gcmd.error("Invalid MODEL='%s' choice" % (pa_model_name,)) + if pa_model_name != self.pa_model.name: + pa_model = self.pa_models[pa_model_name]().get_from_gcmd(gcmd) + else: + pa_model = self.pa_model.get_from_gcmd(gcmd) smooth_time = gcmd.get_float('SMOOTH_TIME', self.pressure_advance_smooth_time, minval=0., maxval=.200) time_offset = gcmd.get_float('TIME_OFFSET', self.pressure_advance_time_offset, minval=-0.2, maxval=0.2) - self._set_pressure_advance(pressure_advance, smooth_time, time_offset) - msg = ("pressure_advance: %.6f\n" - "pressure_advance_smooth_time: %.6f\n" - "pressure_advance_time_offset: %.6f" - % (pressure_advance, smooth_time, time_offset)) + self._update_pressure_advance(pa_model, smooth_time, time_offset) + msg = ("pressure_advance_model: %s\n" % (pa_model.name,) + + pa_model.get_msg() + + "\npressure_advance_smooth_time: %.6f" + "\npressure_advance_time_offset: %.6f" + % (smooth_time, time_offset)) self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg)) gcmd.respond_info(msg, log=False) From 813723a56c9a6b375052581a44344f74f0ffc062 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Fri, 7 Apr 2023 18:13:24 +0200 Subject: [PATCH 14/35] extruder: Sync extruder motion with input shaping Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 16 ++-- klippy/chelper/kin_extruder.c | 75 ++++++++++++++- klippy/chelper/kin_shaper.c | 26 ++--- klippy/chelper/kin_shaper.h | 17 ++++ klippy/extras/input_shaper.py | 140 ++++++++++++++++++++++----- klippy/kinematics/extruder.py | 175 +++++++++++++++++----------------- 6 files changed, 307 insertions(+), 142 deletions(-) create mode 100644 klippy/chelper/kin_shaper.h diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 8bbd27068..662f6a224 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -41,14 +41,8 @@ ] DEST_LIB = "c_helper.so" OTHER_FILES = [ - "list.h", - "serialqueue.h", - "stepcompress.h", - "itersolve.h", - "pyhelper.h", - "trapq.h", - "pollreactor.h", - "msgblock.h", + 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', + 'trapq.h', 'pollreactor.h', 'msgblock.h', 'kin_shaper.h' ] defs_stepcompress = """ @@ -172,11 +166,13 @@ , double pa_velocity, struct pressure_advance_params *pa_params); void extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk , double (*func)(double, double, struct pressure_advance_params *)); + int extruder_set_shaper_params(struct stepper_kinematics *sk, char axis + , int n, double a[], double t[]); + double extruder_get_step_gen_window(struct stepper_kinematics *sk); """ defs_kin_shaper = """ - double input_shaper_get_step_generation_window( - struct stepper_kinematics *sk); + double input_shaper_get_step_gen_window(struct stepper_kinematics *sk); int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis , int n, double a[], double t[]); int input_shaper_set_sk(struct stepper_kinematics *sk diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 77a1c01b2..f2156128c 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -10,6 +10,7 @@ #include // memset #include "compiler.h" // __visible #include "itersolve.h" // struct stepper_kinematics +#include "kin_shaper.h" // struct shaper_pulses #include "pyhelper.h" // errorf #include "trapq.h" // move_get_distance @@ -76,6 +77,14 @@ static void pa_range_integrate(struct move *m, int axis, double move_time, double hst , double *pos_integral, double *pa_velocity_integral) { + while (unlikely(move_time < 0.)) { + m = list_prev_entry(m, node); + move_time += m->move_t; + } + while (unlikely(move_time > m->move_t)) { + move_time -= m->move_t; + m = list_next_entry(m, node); + } // Calculate integral for the current move *pos_integral = *pa_velocity_integral = 0.; double m_pos_int, m_pa_vel_int; @@ -110,6 +119,24 @@ pa_range_integrate(struct move *m, int axis, double move_time, double hst *pos_integral -= m_pos_int; *pa_velocity_integral -= m_pa_vel_int; } + *pos_integral += start_base * hst * hst; +} + +static void +shaper_pa_range_integrate(struct move *m, int axis, double move_time + , double hst, struct shaper_pulses *sp + , double *pos_integral, double *pa_velocity_integral) +{ + *pos_integral = *pa_velocity_integral = 0.; + int num_pulses = sp->num_pulses, i; + for (i = 0; i < num_pulses; ++i) { + double t = sp->pulses[i].t, a = sp->pulses[i].a; + double p_pos_int, p_pa_vel_int; + pa_range_integrate(m, axis, move_time + t, hst, + &p_pos_int, &p_pa_vel_int); + *pos_integral += a * p_pos_int; + *pa_velocity_integral += a * p_pa_vel_int; + } } struct pressure_advance_params { @@ -129,6 +156,7 @@ typedef double (*pressure_advance_func)( struct extruder_stepper { struct stepper_kinematics sk; + struct shaper_pulses sp[3]; struct pressure_advance_params pa_params; pressure_advance_func pa_func; double time_offset, half_smooth_time, inv_half_smooth_time2; @@ -184,16 +212,25 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m struct coord e_pos, pa_vel; double move_dist = move_get_distance(m, move_time); for (i = 0; i < 3; ++i) { + int axis = 'x' + i; + struct shaper_pulses* sp = &es->sp[i]; + int num_pulses = sp->num_pulses; if (!hst) { - e_pos.axis[i] = m->axes_r.axis[i] * move_dist; + e_pos.axis[i] = num_pulses + ? shaper_calc_position(m, axis, move_time, sp) + : m->start_pos.axis[i] + m->axes_r.axis[i] * move_dist; pa_vel.axis[i] = 0.; } else { - pa_range_integrate(m, 'x' + i, move_time, hst, - &e_pos.axis[i], &pa_vel.axis[i]); + if (num_pulses) { + shaper_pa_range_integrate(m, axis, move_time, hst, sp, + &e_pos.axis[i], &pa_vel.axis[i]); + } else { + pa_range_integrate(m, axis, move_time, hst, + &e_pos.axis[i], &pa_vel.axis[i]); + } e_pos.axis[i] *= es->inv_half_smooth_time2; pa_vel.axis[i] *= es->inv_half_smooth_time2; } - e_pos.axis[i] += m->start_pos.axis[i]; } double position = e_pos.x + e_pos.y + e_pos.z; if (!hst) @@ -207,6 +244,15 @@ static void extruder_note_generation_time(struct extruder_stepper *es) { double pre_active = 0., post_active = 0.; + int i; + for (i = 0; i < 2; ++i) { + struct shaper_pulses* sp = &es->sp[i]; + if (!es->sp[i].num_pulses) continue; + pre_active = sp->pulses[sp->num_pulses-1].t > pre_active + ? sp->pulses[sp->num_pulses-1].t : pre_active; + post_active = -sp->pulses[0].t > post_active + ? -sp->pulses[0].t : post_active; + } pre_active += es->half_smooth_time + es->time_offset; if (pre_active < 0.) pre_active = 0.; post_active += es->half_smooth_time - es->time_offset; @@ -243,6 +289,27 @@ extruder_set_pressure_advance_model_func(struct stepper_kinematics *sk es->pa_func = func; } +int __visible +extruder_set_shaper_params(struct stepper_kinematics *sk, char axis + , int n, double a[], double t[]) +{ + if (axis != 'x' && axis != 'y') + return -1; + struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + struct shaper_pulses *sp = &es->sp[axis-'x']; + int status = init_shaper(n, a, t, sp); + extruder_note_generation_time(es); + return status; +} + +double __visible +extruder_get_step_gen_window(struct stepper_kinematics *sk) +{ + struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + return es->sk.gen_steps_pre_active > es->sk.gen_steps_post_active + ? es->sk.gen_steps_pre_active : es->sk.gen_steps_post_active; +} + struct stepper_kinematics * __visible extruder_stepper_alloc(void) { diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c index 18d141439..6a2f9ec71 100644 --- a/klippy/chelper/kin_shaper.c +++ b/klippy/chelper/kin_shaper.c @@ -11,6 +11,7 @@ #include // memset #include "compiler.h" // __visible #include "itersolve.h" // struct stepper_kinematics +#include "kin_shaper.h" // struct shaper_pulses #include "trapq.h" // struct move @@ -18,13 +19,6 @@ * Shaper initialization ****************************************************************/ -struct shaper_pulses { - int num_pulses; - struct { - double t, a; - } pulses[5]; -}; - // Shift pulses around 'mid-point' t=0 so that the input shaper is an identity // transformation for constant-speed motion (i.e. input_shaper(v * T) = v * T) static void @@ -38,7 +32,7 @@ shift_pulses(struct shaper_pulses *sp) sp->pulses[i].t -= ts; } -static int +int init_shaper(int n, double a[], double t[], struct shaper_pulses *sp) { if (n < 0 || n > ARRAY_SIZE(sp->pulses)) { @@ -89,9 +83,9 @@ get_axis_position_across_moves(struct move *m, int axis, double time) } // Calculate the position from the convolution of the shaper with input signal -static inline double -calc_position(struct move *m, int axis, double move_time - , struct shaper_pulses *sp) +inline double +shaper_calc_position(struct move *m, int axis, double move_time + , struct shaper_pulses *sp) { double res = 0.; int num_pulses = sp->num_pulses, i; @@ -124,7 +118,7 @@ shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m struct input_shaper *is = container_of(sk, struct input_shaper, sk); if (!is->sx.num_pulses) return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); - is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx); + is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -136,7 +130,7 @@ shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m struct input_shaper *is = container_of(sk, struct input_shaper, sk); if (!is->sy.num_pulses) return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); - is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy); + is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -150,9 +144,9 @@ shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); is->m.start_pos = move_get_coord(m, move_time); if (is->sx.num_pulses) - is->m.start_pos.x = calc_position(m, 'x', move_time, &is->sx); + is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx); if (is->sy.num_pulses) - is->m.start_pos.y = calc_position(m, 'y', move_time, &is->sy); + is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -213,7 +207,7 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis } double __visible -input_shaper_get_step_generation_window(struct stepper_kinematics *sk) +input_shaper_get_step_gen_window(struct stepper_kinematics *sk) { struct input_shaper *is = container_of(sk, struct input_shaper, sk); return is->sk.gen_steps_pre_active > is->sk.gen_steps_post_active diff --git a/klippy/chelper/kin_shaper.h b/klippy/chelper/kin_shaper.h new file mode 100644 index 000000000..16012c4a4 --- /dev/null +++ b/klippy/chelper/kin_shaper.h @@ -0,0 +1,17 @@ +#ifndef __KIN_SHAPER_H +#define __KIN_SHAPER_H + +struct shaper_pulses { + int num_pulses; + struct { + double t, a; + } pulses[5]; +}; + +struct move; + +int init_shaper(int n, double a[], double t[], struct shaper_pulses *sp); +double shaper_calc_position(struct move *m, int axis, double move_time + , struct shaper_pulses *sp); + +#endif // kin_shaper.h diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index daaa2e683..157bbba84 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -71,8 +71,9 @@ def __init__(self, axis, config): self.saved = None def get_name(self): - return "shaper_" + self.axis - + return 'shaper_' + self.axis + def get_axis(self): + return self.axis def get_shaper(self): return self.n, self.A, self.T @@ -80,8 +81,7 @@ def update(self, gcmd): self.params.update(gcmd) old_n, old_A, old_T = self.n, self.A, self.T self.n, self.A, self.T = self.params.get_shaper() - - def set_shaper_kinematics(self, sk): + def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() success = ( ffi_lib.input_shaper_set_shaper_params( @@ -95,20 +95,30 @@ def set_shaper_kinematics(self, sk): sk, self.axis.encode(), self.n, self.A, self.T ) return success - + def update_extruder_kinematics(self, sk): + ffi_main, ffi_lib = chelper.get_ffi() + success = ffi_lib.extruder_set_shaper_params( + sk, self.axis.encode(), self.n, self.A, self.T) == 0 + if not success: + self.disable_shaping() + ffi_lib.extruder_set_shaper_params( + sk, self.axis.encode(), self.n, self.A, self.T) + return success def disable_shaping(self): + was_enabled = False if self.saved is None and self.n: self.saved = (self.n, self.A, self.T) + was_enabled = True A, T = shaper_defs.get_none_shaper() self.n, self.A, self.T = len(A), A, T - + return was_enabled def enable_shaping(self): if self.saved is None: # Input shaper was not disabled - return + return False self.n, self.A, self.T = self.saved self.saved = None - + return True def report(self, gcmd): info = " ".join( [ @@ -124,25 +134,34 @@ def __init__(self, config): self.printer = config.get_printer() self.printer.register_event_handler("klippy:connect", self.connect) self.toolhead = None - self.shapers = [ - AxisInputShaper("x", config), - AxisInputShaper("y", config), - ] + self.extruders = [] + self.config_extruder_names = config.getlist('enabled_extruders', []) + self.shapers = [AxisInputShaper('x', config), + AxisInputShaper('y', config)] self.input_shaper_stepper_kinematics = [] self.orig_stepper_kinematics = [] # Register gcode commands - gcode = self.printer.lookup_object("gcode") - gcode.register_command( - "SET_INPUT_SHAPER", - self.cmd_SET_INPUT_SHAPER, - desc=self.cmd_SET_INPUT_SHAPER_help, - ) - + gcode = self.printer.lookup_object('gcode') + gcode.register_command("SET_INPUT_SHAPER", + self.cmd_SET_INPUT_SHAPER, + desc=self.cmd_SET_INPUT_SHAPER_help) + gcode.register_command("ENABLE_INPUT_SHAPER", + self.cmd_ENABLE_INPUT_SHAPER, + desc=self.cmd_ENABLE_INPUT_SHAPER_help) + gcode.register_command("DISABLE_INPUT_SHAPER", + self.cmd_DISABLE_INPUT_SHAPER, + desc=self.cmd_DISABLE_INPUT_SHAPER_help) def get_shapers(self): return self.shapers def connect(self): self.toolhead = self.printer.lookup_object("toolhead") + for en in self.config_extruder_names: + extruder = self.printer.lookup_object(en) + if not hasattr(extruder, 'get_extruder_steppers'): + raise self.printer.config_error( + "Invalid extruder '%s' in [input_shaper]" % (en,)) + self.extruders.append(extruder) # Configure initial values self._update_input_shaping(error=self.printer.config_error) @@ -176,17 +195,19 @@ def _update_input_shaping(self, error=None): is_sk = self._get_input_shaper_stepper_kinematics(s) if is_sk is None: continue - old_delay = ffi_lib.input_shaper_get_step_generation_window(is_sk) + old_delay = ffi_lib.input_shaper_get_step_gen_window(is_sk) for shaper in self.shapers: if shaper in failed_shapers: continue - if not shaper.set_shaper_kinematics(is_sk): + if not shaper.update_stepper_kinematics(is_sk): failed_shapers.append(shaper) - new_delay = ffi_lib.input_shaper_get_step_generation_window(is_sk) + new_delay = ffi_lib.input_shaper_get_step_gen_window(is_sk) if old_delay != new_delay: - self.toolhead.note_step_generation_scan_time( - new_delay, old_delay - ) + self.toolhead.note_step_generation_scan_time(new_delay, + old_delay) + for e in self.extruders: + for es in e.get_extruder_steppers(): + failed_shapers.extend(es.update_input_shaping(self.shapers)) if failed_shapers: error = error or self.printer.command_error raise error( @@ -213,6 +234,75 @@ def cmd_SET_INPUT_SHAPER(self, gcmd): self._update_input_shaping() for shaper in self.shapers: shaper.report(gcmd) + cmd_ENABLE_INPUT_SHAPER_help = "Enable input shaper for given objects" + def cmd_ENABLE_INPUT_SHAPER(self, gcmd): + self.toolhead.flush_step_generation() + axes = gcmd.get('AXIS', '') + msg = '' + for axis_str in axes.split(','): + axis = axis_str.strip().lower() + if not axis: + continue + shapers = [s for s in self.shapers if s.get_axis() == axis] + if not shapers: + raise gcmd.error("Invalid AXIS='%s'" % (axis_str,)) + for s in shapers: + if s.enable_shaping(): + msg += "Enabled input shaper for AXIS='%s'\n" % (axis_str,) + else: + msg += ("Cannot enable input shaper for AXIS='%s': " + "was not disabled\n" % (axis_str,)) + extruders = gcmd.get('EXTRUDER', '') + for en in extruders.split(','): + extruder_name = en.strip() + if not extruder_name: + continue + extruder = self.printer.lookup_object(extruder_name) + if not hasattr(extruder, 'get_extruder_steppers'): + raise gcmd.error("Invalid EXTRUDER='%s'" % (en,)) + if extruder not in self.extruders: + self.extruders.append(extruder) + msg += "Enabled input shaper for '%s'\n" % (en,) + else: + msg += "Input shaper already enabled for '%s'\n" % (en,) + self._update_input_shaping() + gcmd.respond_info(msg) + cmd_DISABLE_INPUT_SHAPER_help = "Disable input shaper for given objects" + def cmd_DISABLE_INPUT_SHAPER(self, gcmd): + self.toolhead.flush_step_generation() + axes = gcmd.get('AXIS', '') + msg = '' + for axis_str in axes.split(','): + axis = axis_str.strip().lower() + if not axis: + continue + shapers = [s for s in self.shapers if s.get_axis() == axis] + if not shapers: + raise gcmd.error("Invalid AXIS='%s'" % (axis_str,)) + for s in shapers: + if s.disable_shaping(): + msg += "Disabled input shaper for AXIS='%s'\n" % (axis_str,) + else: + msg += ("Cannot disable input shaper for AXIS='%s': not " + "enabled or was already disabled\n" % (axis_str,)) + extruders = gcmd.get('EXTRUDER', '') + for en in extruders.split(','): + extruder_name = en.strip() + if not extruder_name: + continue + extruder = self.printer.lookup_object(extruder_name) + if extruder in self.extruders: + to_re_enable = [s for s in self.shapers if s.disable_shaping()] + for es in extruder.get_extruder_steppers(): + es.update_input_shaping(self.shapers) + for shaper in to_re_enable: + shaper.enable_shaping() + self.extruders.remove(extruder) + msg += "Disabled input shaper for '%s'\n" % (en,) + else: + msg += "Input shaper not enabled for '%s'\n" % (en,) + self._update_input_shaping() + gcmd.respond_info(msg) def load_config(config): diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 3f174fc9c..2e4b306bb 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -8,18 +8,15 @@ class PALinearModel: name = 'linear' - def __init__(self, config=None, other=None): - self.pressure_advance = 0. + def __init__(self, config=None): if config: self.pressure_advance = config.getfloat( - 'pressure_advance', self.pressure_advance, minval=0.) - elif other: - self.pressure_advance = other.pressure_advance - def get_from_gcmd(self, gcmd): - new_model = PALinearModel(config=None, other=self) - new_model.pressure_advance = gcmd.get_float( + 'pressure_advance', 0., minval=0.) + else: + self.pressure_advance = 0. + def update(self, gcmd): + self.pressure_advance = gcmd.get_float( 'ADVANCE', self.pressure_advance, minval=0.) - return new_model def enabled(self): return self.pressure_advance > 0. def get_pa_params(self): @@ -33,10 +30,7 @@ def get_func(self): return ffi_lib.pressure_advance_linear_model_func class PANonLinearModel: - def __init__(self, config=None, other=None): - self.linear_advance = 0. - self.linear_offset = 0. - self.linearization_velocity = 0. + def __init__(self, config=None): if config: self.linear_advance = config.getfloat('linear_advance', 0., minval=0.) @@ -48,22 +42,20 @@ def __init__(self, config=None, other=None): else: self.linearization_velocity = config.getfloat( 'linearization_velocity', 0., minval=0.) - elif other: - self.linear_advance = other.linear_advance - self.linear_offset = other.linear_offset - self.linearization_velocity = other.linearization_velocity - def get_from_gcmd(self, gcmd): - new_model = self.__class__(config=None, other=self) - new_model.linear_advance = gcmd.get_float( + else: + self.linear_advance = 0. + self.linear_offset = 0. + self.linearization_velocity = 0. + def update(self, gcmd): + self.linear_advance = gcmd.get_float( 'ADVANCE', self.linear_advance, minval=0.) - new_model.linear_offset = gcmd.get_float( + self.linear_offset = gcmd.get_float( 'OFFSET', self.linear_offset, minval=0.) - new_model.linearization_velocity = gcmd.get_float( + self.linearization_velocity = gcmd.get_float( 'VELOCITY', self.linearization_velocity) - if new_model.linear_offset and new_model.linearization_velocity <= 0.: + if self.linear_offset and self.linearization_velocity <= 0.: raise gcmd.error('VELOCITY must be set to a positive value ' 'when OFFSET is non-zero') - return new_model def enabled(self): return self.linear_advance > 0. or self.linear_offset > 0. def get_pa_params(self): @@ -87,16 +79,16 @@ def get_func(self): class PATanhModel(PANonLinearModel): name = 'tanh' - def __init__(self, config=None, other=None): - PANonLinearModel.__init__(self, config, other) + def __init__(self, config=None): + PANonLinearModel.__init__(self, config) def get_func(self): ffi_main, ffi_lib = chelper.get_ffi() return ffi_lib.pressure_advance_tanh_model_func class PAReciprModel(PANonLinearModel): name = 'recipr' - def __init__(self, config=None, other=None): - PANonLinearModel.__init__(self, config, other) + def __init__(self, config=None): + PANonLinearModel.__init__(self, config) def get_func(self): ffi_main, ffi_lib = chelper.get_ffi() return ffi_lib.pressure_advance_recipr_model_func @@ -111,11 +103,9 @@ def __init__(self, config): self.pa_model = config.getchoice('pressure_advance_model', self.pa_models, PALinearModel.name)(config) - self.pressure_advance_smooth_time = 0. - self.pressure_advance_time_offset = 0. - self.config_smooth_time = config.getfloat( + self.pressure_advance_smooth_time = config.getfloat( 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) - self.config_time_offset = config.getfloat( + self.pressure_advance_time_offset = config.getfloat( 'pressure_advance_time_offset', 0.0, minval=-0.2, maxval=0.2) # Setup stepper self.stepper = stepper.PrinterStepper(config) @@ -127,6 +117,7 @@ def __init__(self, config): ffi_lib.extruder_set_pressure_advance_model_func( self.sk_extruder, self.pa_model.get_func()) self.motion_queue = None + self.extruder = None # Register commands self.printer.register_event_handler( "klippy:connect", self._handle_connect @@ -177,11 +168,11 @@ def __init__(self, config): ) def _handle_connect(self): - toolhead = self.printer.lookup_object("toolhead") - toolhead.register_step_generator(self.stepper.generate_steps) + self.toolhead = self.printer.lookup_object('toolhead') + self.toolhead.register_step_generator(self.stepper.generate_steps) self._update_pressure_advance(self.pa_model, - self.config_smooth_time, - self.config_time_offset) + self.pressure_advance_smooth_time, + self.pressure_advance_time_offset) def get_status(self, eventtime): sts = {'pressure_advance_model': self.pa_model.name, 'smooth_time': self.pressure_advance_smooth_time, @@ -194,32 +185,24 @@ def find_past_position(self, print_time): return self.stepper.mcu_to_commanded_position(mcu_pos) def sync_to_extruder(self, extruder_name): - toolhead = self.printer.lookup_object("toolhead") - toolhead.flush_step_generation() + self.toolhead.flush_step_generation() if not extruder_name: - self.stepper.set_trapq(None) + if self.extruder is not None: + self.extruder.unlink_extruder_stepper(self) self.motion_queue = None + self.extruder = None return extruder = self.printer.lookup_object(extruder_name, None) if extruder is None or not isinstance(extruder, PrinterExtruder): raise self.printer.command_error("'%s' is not a valid extruder." % (extruder_name,)) - self.stepper.set_position(extruder.last_position) - self.stepper.set_trapq(extruder.get_trapq()) + extruder.link_extruder_stepper(self) self.motion_queue = extruder_name + self.extruder = extruder def _update_pressure_advance(self, pa_model, smooth_time, time_offset): - old_smooth_time = self.pressure_advance_smooth_time - if not self.pa_model.enabled(): - old_smooth_time = 0. - old_time_offset = self.pressure_advance_time_offset - new_smooth_time = smooth_time - if not pa_model.enabled(): - new_smooth_time = 0. - toolhead = self.printer.lookup_object("toolhead") - toolhead.note_step_generation_scan_time( - new_smooth_time * .5 + abs(time_offset), - old_delay=(old_smooth_time * .5 + abs(old_time_offset))) + self.toolhead.flush_step_generation() ffi_main, ffi_lib = chelper.get_ffi() + old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) if self.pa_model.name != pa_model.name: pa_func = pa_model.get_func() ffi_lib.extruder_set_pressure_advance_model_func(self.sk_extruder, @@ -227,29 +210,44 @@ def _update_pressure_advance(self, pa_model, smooth_time, time_offset): pa_params = pa_model.get_pa_params() ffi_lib.extruder_set_pressure_advance( self.sk_extruder, len(pa_params), pa_params, - new_smooth_time, time_offset) + smooth_time, time_offset) + new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) + if old_delay != new_delay: + self.toolhead.note_step_generation_scan_time(new_delay, old_delay) self.pa_model = pa_model self.pressure_advance_smooth_time = smooth_time self.pressure_advance_time_offset = time_offset + def update_input_shaping(self, shapers): + ffi_main, ffi_lib = chelper.get_ffi() + old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) + failed_shapers = [] + for shaper in self.shapers: + if not shaper.update_extruder_kinematics(self.sk_extruder): + failed_shapers.append(shaper) + new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) + if old_delay != new_delay: + self.toolhead.note_step_generation_scan_time(new_delay, old_delay) + return failed_shapers cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): - extruder = self.printer.lookup_object("toolhead").get_extruder() - if extruder.extruder_stepper is None: + extruder = self.printer.lookup_object('toolhead').get_extruder() + extruder_steppers = extruder.get_extruder_steppers() + if not extruder_steppers: raise gcmd.error("Active extruder does not have a stepper") - strapq = extruder.extruder_stepper.stepper.get_trapq() - if strapq is not extruder.get_trapq(): - raise gcmd.error("Unable to infer active extruder stepper") - extruder.extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) - + for extruder_stepper in extruder_steppers: + strapq = extruder_stepper.stepper.get_trapq() + if strapq is not extruder.get_trapq(): + raise gcmd.error("Unable to infer active extruder stepper") + extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) def cmd_SET_PRESSURE_ADVANCE(self, gcmd): pa_model_name = gcmd.get('MODEL', self.pa_model.name) if pa_model_name not in self.pa_models: raise gcmd.error("Invalid MODEL='%s' choice" % (pa_model_name,)) + pa_model = self.pa_model if pa_model_name != self.pa_model.name: - pa_model = self.pa_models[pa_model_name]().get_from_gcmd(gcmd) - else: - pa_model = self.pa_model.get_from_gcmd(gcmd) + pa_model = self.pa_models[pa_model_name]() + pa_model.update(gcmd) smooth_time = gcmd.get_float('SMOOTH_TIME', self.pressure_advance_smooth_time, minval=0., maxval=.200) @@ -377,36 +375,39 @@ def __init__(self, config, extruder_num): self.trapq_append = ffi_lib.trapq_append self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves # Setup extruder stepper - self.extruder_stepper = None - if ( - config.get("step_pin", None) is not None - or config.get("dir_pin", None) is not None - or config.get("rotation_distance", None) is not None - ): - self.extruder_stepper = ExtruderStepper(config) - self.extruder_stepper.stepper.set_trapq(self.trapq) + self.extruder_steppers = [] + if (config.get('step_pin', None) is not None + or config.get('dir_pin', None) is not None + or config.get('rotation_distance', None) is not None): + self.link_extruder_stepper(ExtruderStepper(config)) # Register commands gcode = self.printer.lookup_object("gcode") if self.name == "extruder": toolhead.set_extruder(self, 0.0) gcode.register_command("M104", self.cmd_M104) gcode.register_command("M109", self.cmd_M109) - gcode.register_mux_command( - "ACTIVATE_EXTRUDER", - "EXTRUDER", - self.name, - self.cmd_ACTIVATE_EXTRUDER, - desc=self.cmd_ACTIVATE_EXTRUDER_help, - ) - + gcode.register_mux_command("ACTIVATE_EXTRUDER", "EXTRUDER", + self.name, self.cmd_ACTIVATE_EXTRUDER, + desc=self.cmd_ACTIVATE_EXTRUDER_help) + def link_extruder_stepper(self, extruder_stepper): + if extruder_stepper not in self.extruder_steppers: + self.extruder_steppers.append(extruder_stepper) + extruder_stepper.stepper.set_position(self.last_position) + extruder_stepper.stepper.set_trapq(self.trapq) + def unlink_extruder_stepper(self, extruder_stepper): + if extruder_stepper in self.extruder_steppers: + self.extruder_steppers.remove(extruder_stepper) + extruder_stepper.stepper.set_trapq(None) + def get_extruder_steppers(self): + return self.extruder_steppers def update_move_time(self, flush_time): self.trapq_finalize_moves(self.trapq, flush_time) def get_status(self, eventtime): sts = self.heater.get_status(eventtime) - sts["can_extrude"] = self.heater.can_extrude - if self.extruder_stepper is not None: - sts.update(self.extruder_stepper.get_status(eventtime)) + sts['can_extrude'] = self.heater.can_extrude + if self.extruder_steppers: + sts.update(self.extruder_steppers[0].get_status(eventtime)) return sts def get_name(self): @@ -487,10 +488,9 @@ def move(self, print_time, move): for i in range(3): self.last_position[i] += extr_d * extr_r[i] def find_past_position(self, print_time): - if self.extruder_stepper is None: - return 0.0 - return self.extruder_stepper.find_past_position(print_time) - + if not self.extruder_steppers: + return 0. + return self.extruder_steppers[0].find_past_position(print_time) def cmd_M104(self, gcmd, wait=False): # Set Extruder Temperature temp = gcmd.get_float("S", 0.0) @@ -545,7 +545,8 @@ def calc_junction(self, prev_move, move): def get_name(self): return "" - + def get_extruder_steppers(self): + return [] def get_heater(self): raise self.printer.command_error("Extruder not configured") From 9661a231a20ccb13df4443b338799832e3e7398a Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Fri, 7 Apr 2023 19:23:35 +0200 Subject: [PATCH 15/35] motan: Report queued steps in extended format Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 2 +- klippy/chelper/stepcompress.c | 1 + klippy/chelper/stepcompress.h | 2 +- klippy/extras/motion_report.py | 38 ++++++++-------------------- scripts/motan/readlog.py | 46 +++++++++++++++++++++------------- 5 files changed, 43 insertions(+), 46 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 662f6a224..7dd2808ab 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -49,7 +49,7 @@ struct pull_history_steps { uint64_t first_clock, last_clock; int64_t start_position; - int step_count, interval, add; + int step_count, interval, add, add2, shift; }; struct stepcompress *stepcompress_alloc(uint32_t oid); diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index e261f1d30..772233d70 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -641,6 +641,7 @@ stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p p->step_count = hs->step_count; p->interval = hs->interval; p->add = hs->add; + p->add2 = p->shift = 0; p++; res++; } diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index b8a950575..98b61c255 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -8,7 +8,7 @@ struct pull_history_steps { uint64_t first_clock, last_clock; int64_t start_position; - int step_count, interval, add; + int step_count, interval, add, add2, shift; }; struct stepcompress *stepcompress_alloc(uint32_t oid); diff --git a/klippy/extras/motion_report.py b/klippy/extras/motion_report.py index 5acdc47f3..85878bc1c 100644 --- a/klippy/extras/motion_report.py +++ b/klippy/extras/motion_report.py @@ -160,19 +160,10 @@ def log_steps(self, data): ) ) for i, s in enumerate(data): - out.append( - "queue_step %d: t=%d p=%d i=%d c=%d a=%d" - % ( - i, - s.first_clock, - s.start_position, - s.interval, - s.step_count, - s.add, - ) - ) - logging.info("\n".join(out)) - + out.append("queue_step %d: t=%d p=%d i=%d c=%d a=%d a2=%d s=%d" + % (i, s.first_clock, s.start_position, s.interval, + s.step_count, s.add, s.add2, s.shift)) + logging.info('\n'.join(out)) def _api_update(self, eventtime): data, cdata = self.get_step_queue(self.last_api_clock, 1 << 63) if not data: @@ -188,22 +179,15 @@ def _api_update(self, eventtime): step_dist = self.mcu_stepper.get_step_dist() if self.mcu_stepper.get_dir_inverted()[0]: step_dist = -step_dist - d = [(s.interval, s.step_count, s.add) for s in data] - return { - "data": d, - "start_position": start_position, - "start_mcu_position": mcu_pos, - "step_distance": step_dist, - "first_clock": first_clock, - "first_step_time": first_time, - "last_clock": last_clock, - "last_step_time": last_time, - } - + d = [(s.interval, s.step_count, s.add, s.add2, s.shift) for s in data] + return {"data": d, "start_position": start_position, + "start_mcu_position": mcu_pos, "step_distance": step_dist, + "first_clock": first_clock, "first_step_time": first_time, + "last_clock": last_clock, "last_step_time": last_time} def _add_api_client(self, web_request): self.api_dump.add_client(web_request) - hdr = ("interval", "count", "add") - web_request.send({"header": hdr}) + hdr = ('interval', 'count', 'add', 'add2', 'shift') + web_request.send({'header': hdr}) NEVER_TIME = 9999999999999999.0 diff --git a/scripts/motan/readlog.py b/scripts/motan/readlog.py index ff91df480..4196de131 100644 --- a/scripts/motan/readlog.py +++ b/scripts/motan/readlog.py @@ -15,6 +15,12 @@ class error(Exception): # Log data handlers ###################################################################### +def shift_interval(interval, shift): + if shift <= 0: + return interval << -shift + else: + return (interval + (1 << (shift-1))) >> shift + # Log data handlers: {name: class, ...} LogHandlers = {} @@ -238,27 +244,30 @@ def _pull_block(self, req_time): if req_time <= last_time: break # Process block into (time, half_position, position) 3-tuples - first_time = step_time = jmsg["first_step_time"] - first_clock = jmsg["first_clock"] - step_clock = first_clock - jmsg["data"][0][0] - cdiff = jmsg["last_clock"] - first_clock + first_time = step_time = jmsg['first_step_time'] + first_clock = jmsg['first_clock'] + step_clock = first_clock - shift_interval(jmsg['data'][0][0], + jmsg['data'][0][4]) + cdiff = jmsg['last_clock'] - first_clock tdiff = last_time - first_time inv_freq = 0.0 if cdiff: inv_freq = tdiff / cdiff - step_dist = jmsg["step_distance"] - step_pos = jmsg["start_position"] + step_dist = jmsg['step_distance'] + step_pos = jmsg['start_position'] - (step_dist if jmsg['data'][0][1] > 0 + else -step_dist) if not step_data[0][0]: - step_data[0] = (0.0, step_pos, step_pos) - for interval, raw_count, add in jmsg["data"]: + step_data[0] = (0., step_pos, step_pos) + for interval, raw_count, add, add2, shift in jmsg['data']: qs_dist = step_dist count = raw_count if count < 0: qs_dist = -qs_dist count = -count for i in range(count): - step_clock += interval + step_clock += shift_interval(interval, shift) interval += add + add += add2 step_time = first_time + (step_clock - first_clock) * inv_freq step_halfpos = step_pos + 0.5 * qs_dist step_pos += qs_dist @@ -351,26 +360,29 @@ def _pull_block(self, req_time): if req_time <= last_time: break # Process block into (time, position) 2-tuples - first_time = step_time = jmsg["first_step_time"] - first_clock = jmsg["first_clock"] - step_clock = first_clock - jmsg["data"][0][0] - cdiff = jmsg["last_clock"] - first_clock + first_time = step_time = jmsg['first_step_time'] + first_clock = jmsg['first_clock'] + step_clock = first_clock - shift_interval(jmsg['data'][0][0], + jmsg['data'][0][4]) + cdiff = jmsg['last_clock'] - first_clock tdiff = last_time - first_time inv_freq = 0.0 if cdiff: inv_freq = tdiff / cdiff - step_pos = jmsg["start_mcu_position"] + step_pos = jmsg['start_mcu_position'] - (1 if jmsg['data'][0][1] > 0 + else -1) if not step_data[0][0]: - step_data[0] = (0.0, step_pos) - for interval, raw_count, add in jmsg["data"]: + step_data[0] = (0., step_pos) + for interval, raw_count, add, add2, shift in jmsg['data']: qs_dist = 1 count = raw_count if count < 0: qs_dist = -1 count = -count for i in range(count): - step_clock += interval + step_clock += shift_interval(interval, shift) interval += add + add += add2 step_time = first_time + (step_clock - first_clock) * inv_freq step_pos += qs_dist step_data.append((step_time, step_pos)) From fe1304a114a1f676bcf2395487b2ccd7b6e70e48 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Fri, 7 Apr 2023 20:49:23 +0200 Subject: [PATCH 16/35] stepper: New optional high precision stepping protocol This enables an extended processing of the steps next_step = step + (interval + rounding) >> shift; interval += add; add += add2; Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 25 +- klippy/chelper/stepcompress.c | 99 +++-- klippy/chelper/stepcompress.h | 37 ++ klippy/chelper/stepcompress_hp.c | 621 +++++++++++++++++++++++++++++++ klippy/stepper.py | 71 ++-- src/stepper.c | 191 +++++++++- 6 files changed, 915 insertions(+), 129 deletions(-) create mode 100644 klippy/chelper/stepcompress_hp.c diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 7dd2808ab..50cf1a641 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -19,25 +19,11 @@ ) SSE_FLAGS = "-mfpmath=sse -msse2" SOURCE_FILES = [ - "pyhelper.c", - "serialqueue.c", - "stepcompress.c", - "itersolve.c", - "trapq.c", - "pollreactor.c", - "msgblock.c", - "trdispatch.c", - "kin_cartesian.c", - "kin_corexy.c", - "kin_corexz.c", - "kin_delta.c", - "kin_deltesian.c", - "kin_polar.c", - "kin_rotary_delta.c", - "kin_winch.c", - "kin_extruder.c", - "kin_shaper.c", - "kin_idex.c", + 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'stepcompress_hp.c', + 'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c', + 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', + 'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', + 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', ] DEST_LIB = "c_helper.so" OTHER_FILES = [ @@ -53,6 +39,7 @@ }; struct stepcompress *stepcompress_alloc(uint32_t oid); + struct stepcompress *stepcompress_hp_alloc(uint32_t oid); void stepcompress_fill(struct stepcompress *sc, uint32_t max_error , int32_t queue_step_msgtag, int32_t set_next_step_dir_msgtag); void stepcompress_set_invert_sdir(struct stepcompress *sc diff --git a/klippy/chelper/stepcompress.c b/klippy/chelper/stepcompress.c index 772233d70..edddd0403 100644 --- a/klippy/chelper/stepcompress.c +++ b/klippy/chelper/stepcompress.c @@ -28,26 +28,6 @@ #define CHECK_LINES 1 #define QUEUE_START_SIZE 1024 -struct stepcompress { - // Buffer management - uint32_t *queue, *queue_end, *queue_pos, *queue_next; - // Internal tracking - uint32_t max_error; - double mcu_time_offset, mcu_freq, last_step_print_time; - // Message generation - uint64_t last_step_clock; - struct list_head msg_queue; - uint32_t oid; - int32_t queue_step_msgtag, set_next_step_dir_msgtag; - int sdir, invert_sdir; - // Step+dir+step filter - uint64_t next_step_clock; - int next_step_dir; - // History tracking - int64_t last_position; - struct list_head history_list; -}; - struct step_move { uint32_t interval; uint16_t count; @@ -56,14 +36,6 @@ struct step_move { #define HISTORY_EXPIRE (30.0) -struct history_steps { - struct list_node node; - uint64_t first_clock, last_clock; - int64_t start_position; - int step_count, interval, add; -}; - - /**************************************************************** * Step compression ****************************************************************/ @@ -243,6 +215,9 @@ check_line(struct stepcompress *sc, struct step_move move) * Step compress interface ****************************************************************/ +int queue_flush(struct stepcompress *sc, uint64_t move_clock); +int queue_flush_far(struct stepcompress *sc, uint64_t abs_step_clock); + // Allocate a new 'stepcompress' object struct stepcompress * __visible stepcompress_alloc(uint32_t oid) @@ -253,6 +228,8 @@ stepcompress_alloc(uint32_t oid) list_init(&sc->history_list); sc->oid = oid; sc->sdir = -1; + sc->queue_flush = queue_flush; + sc->queue_flush_far = queue_flush_far; return sc; } @@ -298,6 +275,8 @@ stepcompress_free(struct stepcompress *sc) { if (!sc) return; + free(sc->rhs_cache); + free(sc->errb_cache); free(sc->queue); message_queue_free(&sc->msg_queue); free_history(sc, UINT64_MAX); @@ -317,8 +296,8 @@ stepcompress_get_step_dir(struct stepcompress *sc) } // Determine the "print time" of the last_step_clock -static void -calc_last_step_print_time(struct stepcompress *sc) +void +stepcompress_calc_last_step_print_time(struct stepcompress *sc) { double lsc = sc->last_step_clock; sc->last_step_print_time = sc->mcu_time_offset + (lsc - .5) / sc->mcu_freq; @@ -334,7 +313,7 @@ stepcompress_set_time(struct stepcompress *sc { sc->mcu_time_offset = time_offset; sc->mcu_freq = mcu_freq; - calc_last_step_print_time(sc); + stepcompress_calc_last_step_print_time(sc); } // Maximium clock delta between messages in the queue @@ -366,13 +345,14 @@ add_move(struct stepcompress *sc, uint64_t first_clock, struct step_move *move) hs->start_position = sc->last_position; hs->interval = move->interval; hs->add = move->add; + hs->add2 = hs->shift = 0; hs->step_count = sc->sdir ? move->count : -move->count; sc->last_position += hs->step_count; list_add_head(&hs->node, &sc->history_list); } // Convert previously scheduled steps into commands for the mcu -static int +int queue_flush(struct stepcompress *sc, uint64_t move_clock) { if (sc->queue_pos >= sc->queue_next) @@ -391,17 +371,17 @@ queue_flush(struct stepcompress *sc, uint64_t move_clock) } sc->queue_pos += move.count; } - calc_last_step_print_time(sc); + stepcompress_calc_last_step_print_time(sc); return 0; } // Generate a queue_step for a step far in the future from the last step -static int -stepcompress_flush_far(struct stepcompress *sc, uint64_t abs_step_clock) +int +queue_flush_far(struct stepcompress *sc, uint64_t abs_step_clock) { struct step_move move = { abs_step_clock - sc->last_step_clock, 1, 0 }; add_move(sc, abs_step_clock, &move); - calc_last_step_print_time(sc); + stepcompress_calc_last_step_print_time(sc); return 0; } @@ -411,7 +391,7 @@ set_next_step_dir(struct stepcompress *sc, int sdir) { if (sc->sdir == sdir) return 0; - int ret = queue_flush(sc, UINT64_MAX); + int ret = sc->queue_flush(sc, UINT64_MAX); if (ret) return ret; sc->sdir = sdir; @@ -430,11 +410,11 @@ queue_append_far(struct stepcompress *sc) { uint64_t step_clock = sc->next_step_clock; sc->next_step_clock = 0; - int ret = queue_flush(sc, step_clock - CLOCK_DIFF_MAX + 1); + int ret = sc->queue_flush(sc, step_clock - CLOCK_DIFF_MAX + 1); if (ret) return ret; if (step_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) - return stepcompress_flush_far(sc, step_clock); + return sc->queue_flush_far(sc, step_clock); *sc->queue_next++ = step_clock; return 0; } @@ -447,7 +427,7 @@ queue_append_extend(struct stepcompress *sc) // No point in keeping more than 64K steps in memory uint32_t flush = (*(sc->queue_next-65535) - (uint32_t)sc->last_step_clock); - int ret = queue_flush(sc, sc->last_step_clock + flush); + int ret = sc->queue_flush(sc, sc->last_step_clock + flush); if (ret) return ret; } @@ -545,7 +525,7 @@ stepcompress_flush(struct stepcompress *sc, uint64_t move_clock) if (ret) return ret; } - return queue_flush(sc, move_clock); + return sc->queue_flush(sc, move_clock); } // Reset the internal state of the stepcompress object @@ -557,7 +537,7 @@ stepcompress_reset(struct stepcompress *sc, uint64_t last_step_clock) return ret; sc->last_step_clock = last_step_clock; sc->sdir = -1; - calc_last_step_print_time(sc); + stepcompress_calc_last_step_print_time(sc); return 0; } @@ -593,15 +573,33 @@ stepcompress_find_past_position(struct stepcompress *sc, uint64_t clock) } if (clock >= hs->last_clock) return hs->start_position + hs->step_count; - int32_t interval = hs->interval, add = hs->add; - int32_t ticks = (int32_t)(clock - hs->first_clock) + interval, offset; - if (!add) { - offset = ticks / interval; + int64_t ticks = clock - hs->first_clock; + int64_t interval = hs->interval, add = hs->add, add2 = hs->add2; + int count = hs->step_count, shift = hs->shift; + if (count < 0) count = -count; + if (shift <= 0) { + int mul = 1 << -shift; + interval <<= -shift; + add *= mul; + add2 *= mul; } else { - // Solve for "count" using quadratic formula - double a = .5 * add, b = interval - .5 * add, c = -ticks; - offset = (sqrt(b*b - 4*a*c) - b) / (2. * a); + ticks *= 1 << shift; + } + // When clock == hs->first_clock, offset == 1 + ticks += interval; + int left = 0, right = count; + while (right - left > 1) { + int cnt = (left + right) / 2; + int64_t step_clock = cnt * interval + cnt * (cnt-1) / 2 * add + + cnt * (cnt-1) * (cnt-2) / 6 * add2; + if (step_clock <= ticks) left = cnt; + else right = cnt; } + int64_t clock_left = left * interval + left * (left-1) / 2 * add + + left * (left-1) * (left-2) / 6 * add2; + int64_t clock_right = right * interval + right * (right-1) / 2 * add + + right * (right-1) * (right-2) / 6 * add2; + int offset = ticks - clock_left <= clock_right - ticks ? left : right; if (hs->step_count < 0) return hs->start_position - offset; return hs->start_position + offset; @@ -641,7 +639,8 @@ stepcompress_extract_old(struct stepcompress *sc, struct pull_history_steps *p p->step_count = hs->step_count; p->interval = hs->interval; p->add = hs->add; - p->add2 = p->shift = 0; + p->add2 = hs->add2; + p->shift = hs->shift; p++; res++; } diff --git a/klippy/chelper/stepcompress.h b/klippy/chelper/stepcompress.h index 98b61c255..5eb517a3d 100644 --- a/klippy/chelper/stepcompress.h +++ b/klippy/chelper/stepcompress.h @@ -2,9 +2,45 @@ #define STEPCOMPRESS_H #include // uint32_t +#include "list.h" // struct list_head #define ERROR_RET -989898989 +struct stepcompress { + // Buffer management + uint32_t *queue, *queue_end, *queue_pos, *queue_next; + // Internal tracking + uint32_t max_error; + double mcu_time_offset, mcu_freq, last_step_print_time; + // Message generation + uint64_t last_step_clock; + struct list_head msg_queue; + uint32_t oid; + int32_t queue_step_msgtag, set_next_step_dir_msgtag; + int sdir, invert_sdir; + // Step+dir+step filter + uint64_t next_step_clock; + int next_step_dir; + // History tracking + int64_t last_position; + struct list_head history_list; + // Extra fields for high precision algorithm + uint32_t next_expected_interval; + uint16_t cached_count; + struct rhs_3 *rhs_cache; + struct points *errb_cache; + // Callbacks + int (*queue_flush)(struct stepcompress *, uint64_t); + int (*queue_flush_far)(struct stepcompress *, uint64_t); +}; + +struct history_steps { + struct list_node node; + uint64_t first_clock, last_clock; + int64_t start_position; + int step_count, interval, add, add2, shift; +}; + struct pull_history_steps { uint64_t first_clock, last_clock; int64_t start_position; @@ -28,6 +64,7 @@ int stepcompress_set_last_position(struct stepcompress *sc, uint64_t clock , int64_t last_position); int64_t stepcompress_find_past_position(struct stepcompress *sc , uint64_t clock); +void stepcompress_calc_last_step_print_time(struct stepcompress *sc); int stepcompress_queue_msg(struct stepcompress *sc, uint32_t *data, int len); int stepcompress_extract_old(struct stepcompress *sc , struct pull_history_steps *p, int max diff --git a/klippy/chelper/stepcompress_hp.c b/klippy/chelper/stepcompress_hp.c new file mode 100644 index 000000000..eca5ea9d1 --- /dev/null +++ b/klippy/chelper/stepcompress_hp.c @@ -0,0 +1,621 @@ +// Stepper pulse schedule compression +// +// Copyright (C) 2016-2021 Kevin O'Connor +// Copyright (C) 2022-2023 Dmitry Butyugin +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +// The goal of this code is to take a series of scheduled stepper +// pulse times and compress them into a handful of commands that can +// be efficiently transmitted and executed on a microcontroller (mcu). +// The mcu accepts step pulse commands that take interval, count, and +// add parameters such that 'count' pulses occur, with each step event +// calculating the next step event time using: +// next_wake_time = last_wake_time + interval; interval += add +// This code is written in C (instead of python) for processing +// efficiency - the repetitive integer math is vastly faster in C. + +#include // sqrt +#include // offsetof +#include // uint32_t +#include // fprintf +#include // malloc +#include // memset +#include "compiler.h" // DIV_ROUND_UP +#include "pyhelper.h" // errorf +#include "serialqueue.h" // struct queue_message +#include "stepcompress.h" // stepcompress_alloc + +// Maximum error between compressed and actual step, +// in the form of (step_i - step_{i-1}) >> MAX_ERR_2P +#define MAX_ERR_2P 6 +// Minimum tolerable step error in clock ticks. Step errors +// below this value are ignored as they are getting close +// to the integer precision of a single clock tick. +#define MIN_STEP_ERR 3 +// Limits on step_move values for the least squares method +#define MAX_COUNT_LSM 1024 +// Maximum 'count' value for binary search, must be smaller +// than the MAX_COUNT_LSM value +#define MAX_COUNT_BISECT 512 +// Limits below are optimized for "message blocks" encoding +// and underlying storage types and MCU-side implementation +#define MAX_INTRVL 0x3FFFFFF +#define MAX_ADD 0x7FFF +#define MAX_ADD2 0xFFF +#define MAX_SHIFT 16 +#define MIN_SHIFT -8 +#define MAX_INT32 0x7FFFFFFF + +struct step_move_hp { + uint32_t interval; + uint16_t count; + int16_t add; + int16_t add2; + int8_t shift; + uint64_t first_step, last_step; + uint32_t next_step_interval; +}; + +/**************************************************************** + * Step compression + ****************************************************************/ + +// A bias of the first step in the generated step sequence step_move towards +// the first required step in the least squares method. The zero value means +// no bias: the first step error is minimized as all other step errors, and +// values larger than zero make the method push the first generated step +// towards stepcompress.next_expected_interval. This helps reducing the jitter +// between subsequent step_move(s) and pushing back the maximum errors towards +// the end of the generated step_move sequence. +#define FIRST_STEP_BIAS 1.0 +// Extra bias for the first step towards the previously expected step to try +// if the step_move generated using the regular parameters fail early. +#define EXTRA_FIRST_STEP_BIAS 19.0 + +#define A2_REGULARIZATION 0.01 + +struct matrix_3x3 { + double a00, a10, a11, a20, a21, a22; +}; + +struct rhs_3 { + double b0, b1, b2; +}; + +struct matrix_3x3 least_squares_ldl[MAX_COUNT_LSM] = {0}; +struct matrix_3x3 least_squares_efsb_ldl[MAX_COUNT_LSM] = {0}; + +static void +fill_least_squares_matrix_3x3(uint16_t count, struct matrix_3x3 *m) +{ + int32_t i; + memset(m, 0, sizeof(*m)); + for (i = 0; i < count; ++i) { + int32_t c0 = i+1; + int32_t c1 = c0 * i / 2; + int32_t c2 = c1 * (i-1) / 3; + + m->a00 += (double)c0 * c0; + m->a10 += (double)c1 * c0; + m->a11 += (double)c1 * c1; + m->a20 += (double)c2 * c0; + m->a21 += (double)c2 * c1; + m->a22 += (double)c2 * c2; + } + m->a00 += FIRST_STEP_BIAS; + m->a22 += A2_REGULARIZATION; + if (i < 2) m->a11 = 1.; + if (i < 3) m->a22 = 1.; +} + +static void +compute_ldl_3x3(struct matrix_3x3 *m) +{ + double d0 = m->a00; + m->a00 = 1. / d0; + m->a10 *= m->a00; + m->a20 *= m->a00; + + double d1 = m->a11 - d0 * m->a10 * m->a10; + m->a11 = 1. / d1; + m->a21 -= m->a20 * m->a10 * d0; + m->a21 *= m->a11; + + double d2 = m->a22 - d0 * m->a20 * m->a20 - d1 * m->a21 * m->a21; + m->a22 = 1. / d2; +} + +static void +compute_rhs_3(struct stepcompress *sc, uint16_t count, struct rhs_3 *f + , struct rhs_3 *prev_f) +{ + uint32_t lsc = sc->last_step_clock; + if (unlikely(count <= 1)) { + memset(f, 0, sizeof(*f)); + f->b0 += FIRST_STEP_BIAS * sc->next_expected_interval; + } else { + *f = *prev_f; + } + double d = sc->queue_pos[count-1] - lsc; + int32_t c = count; + f->b0 += d * c; + c = c * (count-1) / 2; + f->b1 += d * c; + c = c * (count-2) / 3; + f->b2 += d * c; +} + +static void +solve_3x3(struct matrix_3x3 *m, struct rhs_3 *f) +{ + f->b1 -= f->b0 * m->a10; + f->b2 -= f->b0 * m->a20 + f->b1 * m->a21; + + f->b0 *= m->a00; + f->b1 *= m->a11; + f->b2 *= m->a22; + + f->b1 -= f->b2 * m->a21; + f->b0 -= f->b1 * m->a10 + f->b2 * m->a20; +} + +static struct step_move_hp +step_move_encode(uint16_t count, struct rhs_3* f) +{ + struct step_move_hp res; + memset(&res, 0, sizeof(res)); + double interval = f->b0, add = f->b1, add2 = f->b2; + if (interval < 0. || count > 0x7FFF) + return res; + if (count <= 1) { + res.count = count; + res.interval = round(interval); + return res; + } + double end_add = add + add2 * count; + double max_int_inc = count * (fabs(add) > fabs(end_add) ? + fabs(add) : fabs(end_add)); + double max_end_int = interval + max_int_inc; + if (fabs(add) > MAX_ADD || fabs(end_add) > MAX_ADD || + fabs(add2) > MAX_ADD2 || max_end_int > MAX_INTRVL) { + while (res.shift >= MIN_SHIFT && ( + fabs(add) > MAX_ADD || fabs(end_add) > MAX_ADD || + fabs(add2) > MAX_ADD2 || max_end_int > MAX_INTRVL)) { + interval *= 0.5; + add *= 0.5; + add2 *= 0.5; + end_add *= 0.5; + max_end_int *= 0.5; + --res.shift; + } + if (res.shift < MIN_SHIFT) + // Cannot encode the current rhs_3, the values are too large + return res; + } else if (max_int_inc >= 0.5 || + count * fabs(interval-round(interval)) >= 0.5) { + while (res.shift < MAX_SHIFT) { + double next_interval = interval * 2.; + double next_add = add * 2.; + double next_add2 = add2 * 2.; + double next_end_add = end_add * 2.; + double next_max_end_int = max_end_int * 2.; + int8_t next_shift = res.shift + 1; + uint_fast8_t extra_shift = + next_shift > 8 ? 16-next_shift : 8-next_shift; + if (fabs(next_add) > MAX_ADD || fabs(next_end_add) > MAX_ADD || + fabs(next_add2) > MAX_ADD2 || next_max_end_int > MAX_INTRVL || + next_interval * (1 << extra_shift) > MAX_INT32 || + fabs(next_add * (1 << extra_shift)) > MAX_INT32 || + fabs(next_add2 * (1 << extra_shift)) > MAX_INT32) + break; + interval = next_interval; + add = next_add; + add2 = next_add2; + end_add = next_end_add; + max_end_int = next_max_end_int; + ++res.shift; + } + } + res.count = count; + res.interval = round(interval); + res.add = round(add); + res.add2 = round(add2); + return res; +} + +static struct step_move_hp +single_step_move_encode(uint64_t interval) +{ + struct step_move_hp res; + memset(&res, 0, sizeof(res)); + res.interval = interval; + res.count = 1; + res.first_step = res.last_step = interval; + return res; +} + +struct points { + int32_t minp, maxp; +}; + +// Given a requested step time, return the minimum and maximum +// acceptable times +static inline struct points +minmax_point(struct stepcompress *sc, uint32_t *pos) +{ + uint32_t lsc = sc->last_step_clock, point = *pos - lsc; + + uint32_t max_bck_error = pos > sc->queue ? *pos - *(pos-1) : point; + max_bck_error = (max_bck_error + (1 << (MAX_ERR_2P - 1))) >> MAX_ERR_2P; + if (max_bck_error < MIN_STEP_ERR) max_bck_error = MIN_STEP_ERR; + if (max_bck_error > sc->max_error) max_bck_error = sc->max_error; + + uint32_t max_frw_error = pos + 1 < sc->queue_next ? *(pos+1) - *pos : ( + sc->next_step_clock ? (uint32_t)sc->next_step_clock - *pos : 0); + max_frw_error = (max_frw_error + (1 << (MAX_ERR_2P - 1))) >> MAX_ERR_2P; + if (max_frw_error) { + if (max_frw_error < MIN_STEP_ERR) max_frw_error = MIN_STEP_ERR; + if (max_bck_error > max_frw_error) max_bck_error = max_frw_error; + if (max_frw_error > max_bck_error) max_frw_error = max_bck_error; + } else max_frw_error = MIN_STEP_ERR; + + return (struct points){ point - max_bck_error, point + max_frw_error }; +} + +static inline struct points +get_cached_minmax_point(struct stepcompress *sc, uint16_t ind) +{ + if (ind < MAX_COUNT_LSM && ind < sc->cached_count) + return sc->errb_cache[ind]; + return minmax_point(sc, sc->queue_pos + ind); +} + +struct stepper_moves { + uint32_t interval; + int32_t add; + int32_t add2; + uint_fast8_t shift; + uint16_t int_low_acc; + uint16_t count; +}; + +static inline void +add_interval(uint32_t* time, struct stepper_moves *s) +{ + if (s->shift == 16) { + uint32_t interval = s->interval + s->int_low_acc; + *time += interval >> 16; + s->int_low_acc = interval & 0xFFFF; + } else if (s->shift == 8) { + uint32_t interval = s->interval + s->int_low_acc; + *time += interval >> 8; + s->int_low_acc = interval & 0xFF; + } else { + *time += s->interval; + } +} + +static inline void +inc_interval(struct stepper_moves *s) +{ + s->interval += s->add; + s->add += s->add2; +} + +static void +fill_stepper_moves(struct step_move_hp *m, struct stepper_moves *s) +{ + s->count = m->count; + uint32_t interval = m->interval; + int32_t add = m->add; + int32_t add2 = m->add2; + int8_t shift = m->shift; + + if (shift <= 0) { + interval <<= -shift; + // Left shift of a negative int is an undefined behavior in C + add = add >= 0 ? add << -shift : -(-add << -shift); + add2 = add2 >= 0 ? add2 << -shift : -(-add2 << -shift); + s->interval = interval; + s->add = add; + s->add2 = add2; + s->int_low_acc = 0; + s->shift = 0; + } else { + uint_fast8_t extra_shift = shift > 8 ? 16-shift : 8-shift; + s->shift = shift > 8 ? 16 : 8; + interval <<= extra_shift; + add = add >= 0 ? add << extra_shift : -(-add << extra_shift); + add2 = add2 >= 0 ? add2 << extra_shift : -(-add2 << extra_shift); + s->interval = interval; + s->add = add; + s->add2 = add2; + if (s->shift == 16) { + s->int_low_acc = 1 << 15; + } else { // s->shift == 8 + s->int_low_acc = 1 << 7; + } + } +} + +static int +test_step_move(struct stepcompress *sc, struct step_move_hp *m + , int report_errors, int trunc_move) +{ + if (!m->count || (!m->interval && !m->add && !m->add2 && m->count > 1) + || m->interval >= 0x80000000 + || m->count > 0x7FFF + || m->add < -MAX_ADD || m->add > MAX_ADD + || m->add2 < -MAX_ADD2 || m->add2 > MAX_ADD2 + || m->shift > MAX_SHIFT || m->shift < MIN_SHIFT) { + if (report_errors) + errorf("stepcompress o=%d i=%d c=%d a=%d, a2=%d, s=%d:" + " Invalid sequence" + , sc->oid, m->interval, m->count, m->add, m->add2, m->shift); + m->count = 0; + return ERROR_RET; + } + struct stepper_moves s; + fill_stepper_moves(m, &s); + m->next_step_interval = 0; + uint32_t lsc = sc->last_step_clock; + uint16_t i, trunc_pos = 0; + uint32_t cur_step = 0, prev_step = 0, trunc_last_step = 0; + uint32_t trunc_err = MAX_INT32; + for (i = 0; i < m->count; ++i) { + add_interval(&cur_step, &s); + struct points point = get_cached_minmax_point(sc, i); + if ((int32_t)cur_step < point.minp || cur_step > point.maxp) { + if (report_errors) + errorf("stepcompress o=%d i=%d c=%d a=%d, a2=%d, s=%d:" + " Point %u: %d not in %d:%d" + , sc->oid, m->interval, m->count, m->add, m->add2 + , m->shift, i+1, cur_step, point.minp, point.maxp); + // The least squares method does not minimize the maximum error + // in the generated step sequence, but rather the total error. + // However, we can still use the longest good generated prefix. + m->count = i; + return ERROR_RET; + } + if (trunc_move && m->count > 3 && m->count - i <= (m->count + 9) / 10) { + int32_t err = cur_step - (sc->queue_pos[i] - lsc); + if (err < 0) err = -err; + if (err <= trunc_err || err <= 1) { + trunc_pos = i; + trunc_err = err; + m->next_step_interval = cur_step - prev_step; + trunc_last_step = prev_step; + } + } + inc_interval(&s); + if (s.interval >= 0x80000000) { + if (report_errors) + errorf("stepcompress o=%d i=%d c=%d a=%d, a2=%d, s=%d:" + " Point %d: interval overflow %d" + , sc->oid, m->interval, m->count, m->add, m->add2 + , m->shift, i+1, s.interval); + m->count = i; + return ERROR_RET; + } + m->last_step = cur_step; + if (!m->first_step) + m->first_step = cur_step; + prev_step = cur_step; + } + if (trunc_move && trunc_pos) { + m->count = trunc_pos; + m->last_step = trunc_last_step; + } + return 0; +} + +static struct step_move_hp +test_step_count(struct stepcompress *sc, uint16_t count) +{ + if (count > MAX_COUNT_LSM || sc->cached_count < count) { + struct step_move_hp res; + memset(&res, 0, sizeof(res)); + return res; + } + struct matrix_3x3 *m = &least_squares_ldl[count-1]; + struct rhs_3 f = sc->rhs_cache[count-1]; + solve_3x3(m, &f); + struct step_move_hp res = step_move_encode(count, &f); + test_step_move(sc, &res, /*report_errors=*/0, /*trunc_move=*/0); + if (count > 20 && res.count < count / 4) { + m = &least_squares_efsb_ldl[count-1]; + f = sc->rhs_cache[count-1]; + f.b0 += EXTRA_FIRST_STEP_BIAS * sc->next_expected_interval; + solve_3x3(m, &f); + struct step_move_hp efsb_res = step_move_encode(count, &f); + test_step_move(sc, &efsb_res, /*report_errors=*/0, /*trunc_move=*/0); + if (efsb_res.count > res.count) return efsb_res; + } + return res; +} + +static struct step_move_hp +gen_avg_interval(struct stepcompress *sc, uint16_t count) +{ + uint32_t lsc = sc->last_step_clock; + double d = sc->queue_pos[count-1] - lsc; + d += FIRST_STEP_BIAS * sc->next_expected_interval; + struct rhs_3 f; + memset(&f, 0, sizeof(f)); + f.b0 = d / (count + FIRST_STEP_BIAS); + return step_move_encode(count, &f); +} + +inline static void +update_caches_to_count(struct stepcompress *sc, uint16_t count) +{ + if (!sc->cached_count) { + compute_rhs_3(sc, /*count=*/1, &sc->rhs_cache[0], NULL); + sc->errb_cache[0] = minmax_point(sc, sc->queue_pos); + sc->cached_count = 1; + } + uint16_t i; + for (i = sc->cached_count + 1; i <= count && i <= MAX_COUNT_LSM; ++i) { + compute_rhs_3(sc, i, &sc->rhs_cache[i-1], &sc->rhs_cache[i-2]); + sc->errb_cache[i-1] = minmax_point(sc, sc->queue_pos+i-1); + } + sc->cached_count = i - 1; +} + +// Find a 'step_move_hp' that covers a series of step times +static struct step_move_hp +compress_bisect_count(struct stepcompress *sc) +{ + sc->cached_count = 0; + struct step_move_hp cur, best; + memset(&best, 0, sizeof(best)); + uint16_t queue_size = sc->queue_next < sc->queue_pos + 0x7FFF + ? sc->queue_next - sc->queue_pos : 0x7FFF; + uint16_t left = 0, right = 8; + for (; right <= MAX_COUNT_LSM && right <= queue_size; right <<= 1) { + update_caches_to_count(sc, right); + cur = test_step_count(sc, right); + if (cur.count > best.count) { + best = cur; + left = cur.count; + } + else break; + } + if (right >= MAX_COUNT_BISECT) { + for (; right <= queue_size; right <<= 1) { + cur = gen_avg_interval(sc, right); + test_step_move(sc, &cur, /*report_errors=*/0, /*trunc_move=*/0); + if (cur.count > best.count) best = cur; + else break; + } + return best; + } + if (right > queue_size) right = queue_size + 1; + update_caches_to_count(sc, right - 1); + while (right - left > 1) { + uint16_t count = (left + right) / 2; + cur = test_step_count(sc, count); + if (cur.count > best.count) { + best = cur; + left = count; + } else right = count; + } + if (best.count <= 1) { + uint32_t interval = *sc->queue_pos - (uint32_t)sc->last_step_clock; + return single_step_move_encode(interval); + } + return best; +} + + +/**************************************************************** + * Step compress interface + ****************************************************************/ + +// Maximium clock delta between messages in the queue +#define CLOCK_DIFF_MAX (3<<28) + +// Helper to create a queue_step command from a 'struct step_move_hp' +static void +add_move(struct stepcompress *sc, uint64_t first_clock + , struct step_move_hp *move) +{ + uint64_t last_clock = sc->last_step_clock + move->last_step; + + // Create and queue a queue_step command + uint32_t msg[7] = { + sc->queue_step_msgtag, sc->oid, move->interval, move->count, + move->add, move->add2, move->shift + }; + struct queue_message *qm = message_alloc_and_encode(msg, 7); + qm->min_clock = qm->req_clock = sc->last_step_clock; + if (move->count == 1 && first_clock >= sc->last_step_clock + CLOCK_DIFF_MAX) + qm->req_clock = first_clock; + list_add_tail(&qm->node, &sc->msg_queue); + sc->last_step_clock = last_clock; + sc->next_expected_interval = move->next_step_interval; + + // Create and store move in history tracking + struct history_steps *hs = malloc(sizeof(*hs)); + hs->first_clock = first_clock; + hs->last_clock = last_clock; + hs->start_position = sc->last_position; + hs->interval = move->interval; + hs->add = move->add; + hs->add2 = move->add2; + hs->shift = move->shift; + hs->step_count = sc->sdir ? move->count : -move->count; + sc->last_position += hs->step_count; + list_add_head(&hs->node, &sc->history_list); +} + +// Convert previously scheduled steps into commands for the mcu +int +queue_flush_hp(struct stepcompress *sc, uint64_t move_clock) +{ + if (sc->queue_pos >= sc->queue_next) + return 0; + while (sc->last_step_clock < move_clock) { + if (!sc->next_expected_interval) { + sc->next_expected_interval = + *sc->queue_pos - (uint32_t)sc->last_step_clock; + } + struct step_move_hp move = compress_bisect_count(sc); + // Verify that a given 'step_move_hp' matches the actual step times + // and truncate it to improve the junction with the next step_move_hp + // and reduce velocity jitter if appropriate + int ret = test_step_move(sc, &move, + /*report_errors=*/1, /*trunc_move=*/1); + if (ret) + return ret; + + add_move(sc, sc->last_step_clock + move.first_step, &move); + + if (sc->queue_pos + move.count >= sc->queue_next) { + sc->queue_pos = sc->queue_next = sc->queue; + break; + } + sc->queue_pos += move.count; + } + stepcompress_calc_last_step_print_time(sc); + return 0; +} + +// Generate a queue_step for a step far in the future from the last step +int +queue_flush_far_hp(struct stepcompress *sc, uint64_t abs_step_clock) +{ + uint64_t interval = abs_step_clock - sc->last_step_clock; + struct step_move_hp move = single_step_move_encode(interval); + add_move(sc, abs_step_clock, &move); + stepcompress_calc_last_step_print_time(sc); + return 0; +} + +// Allocate a new 'stepcompress' object +struct stepcompress * __visible +stepcompress_hp_alloc(uint32_t oid) +{ + struct stepcompress *sc = malloc(sizeof(*sc)); + memset(sc, 0, sizeof(*sc)); + list_init(&sc->msg_queue); + list_init(&sc->history_list); + sc->oid = oid; + sc->sdir = -1; + sc->queue_flush = queue_flush_hp; + sc->queue_flush_far = queue_flush_far_hp; + sc->rhs_cache = malloc(sizeof(sc->rhs_cache[0]) * MAX_COUNT_LSM); + sc->errb_cache = malloc(sizeof(sc->errb_cache[0]) * MAX_COUNT_LSM); + if (!least_squares_ldl[0].a00) + for (int i = 0; i < MAX_COUNT_LSM; ++i) { + struct matrix_3x3 *m = &least_squares_ldl[i]; + fill_least_squares_matrix_3x3(i+1, m); + compute_ldl_3x3(m); + m = &least_squares_efsb_ldl[i]; + fill_least_squares_matrix_3x3(i+1, m); + m->a00 += EXTRA_FIRST_STEP_BIAS; + compute_ldl_3x3(m); + } + return sc; +} diff --git a/klippy/stepper.py b/klippy/stepper.py index 77f3b26c5..fd81fb5b5 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -21,17 +21,11 @@ class error(Exception): # Interface to low-level mcu and chelper code class MCU_stepper: - def __init__( - self, - name, - step_pin_params, - dir_pin_params, - rotation_dist, - steps_per_rotation, - step_pulse_duration=None, - units_in_radians=False, - ): + def __init__(self, name, high_precision_steps, step_pin_params, + dir_pin_params, rotation_dist, steps_per_rotation, + step_pulse_duration=None, units_in_radians=False): self._name = name + self._high_precision_steps = high_precision_steps self._rotation_dist = rotation_dist self._steps_per_rotation = steps_per_rotation self._step_pulse_duration = step_pulse_duration @@ -53,9 +47,12 @@ def __init__( self._reset_cmd_tag = self._get_position_cmd = None self._active_callbacks = [] ffi_main, ffi_lib = chelper.get_ffi() - self._stepqueue = ffi_main.gc( - ffi_lib.stepcompress_alloc(oid), ffi_lib.stepcompress_free - ) + if high_precision_steps: + self._stepqueue = ffi_main.gc(ffi_lib.stepcompress_hp_alloc(oid), + ffi_lib.stepcompress_free) + else: + self._stepqueue = ffi_main.gc(ffi_lib.stepcompress_alloc(oid), + ffi_lib.stepcompress_free) ffi_lib.stepcompress_set_invert_sdir(self._stepqueue, self._invert_dir) self._mcu.register_stepqueue(self._stepqueue) self._stepper_kinematics = None @@ -115,21 +112,18 @@ def _build_config(self): step_pulse_ticks = self._mcu.seconds_to_clock(self._step_pulse_duration) self._mcu.add_config_cmd( "config_stepper oid=%d step_pin=%s dir_pin=%s invert_step=%d" - " step_pulse_ticks=%u" - % ( - self._oid, - self._step_pin, - self._dir_pin, - invert_step, - step_pulse_ticks, - ) - ) - self._mcu.add_config_cmd( - "reset_step_clock oid=%d clock=0" % (self._oid,), on_restart=True - ) - step_cmd_tag = self._mcu.lookup_command( - "queue_step oid=%c interval=%u count=%hu add=%hi" - ).get_command_tag() + " step_pulse_ticks=%u" % (self._oid, self._step_pin, self._dir_pin, + invert_step, step_pulse_ticks)) + self._mcu.add_config_cmd("reset_step_clock oid=%d clock=0" + % (self._oid,), on_restart=True) + if self._high_precision_steps: + step_cmd_tag = self._mcu.lookup_command( + "queue_step_hp oid=%c interval=%u count=%hu " + "add=%hi add2=%hi shift=%hi").get_command_tag() + else: + step_cmd_tag = self._mcu.lookup_command( + "queue_step oid=%c interval=%u count=%hu " + "add=%hi").get_command_tag() dir_cmd_tag = self._mcu.lookup_command( "set_next_step_dir oid=%c dir=%c" ).get_command_tag() @@ -313,20 +307,13 @@ def PrinterStepper(config, units_in_radians=False): dir_pin = config.get("dir_pin") dir_pin_params = ppins.lookup_pin(dir_pin, can_invert=True) rotation_dist, steps_per_rotation = parse_step_distance( - config, units_in_radians, True - ) - step_pulse_duration = config.getfloat( - "step_pulse_duration", None, minval=0.0, maxval=0.001 - ) - mcu_stepper = MCU_stepper( - name, - step_pin_params, - dir_pin_params, - rotation_dist, - steps_per_rotation, - step_pulse_duration, - units_in_radians, - ) + config, units_in_radians, True) + step_pulse_duration = config.getfloat('step_pulse_duration', None, + minval=0., maxval=.001) + high_precision_steps = config.getboolean('high_precision_steps', False) + mcu_stepper = MCU_stepper(name, high_precision_steps, step_pin_params, + dir_pin_params, rotation_dist, steps_per_rotation, + step_pulse_duration, units_in_radians) # Register with helper modules for mname in ["stepper_enable", "force_move", "motion_report"]: m = printer.load_object(config, mname) diff --git a/src/stepper.c b/src/stepper.c index 00a8ff012..ae09a59cd 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -31,8 +31,13 @@ struct stepper_move { struct move_node node; + uint32_t first_interval; uint32_t interval; - int16_t add; + // Extra fields for high precision stepping + int32_t add; + int32_t add2; + uint_fast8_t shift; + uint16_t int_low_acc; uint16_t count; uint8_t flags; }; @@ -42,7 +47,10 @@ enum { MF_DIR=1<<0 }; struct stepper { struct timer time; uint32_t interval; - int16_t add; + int32_t add; + int32_t add2; + uint_fast8_t shift; + uint16_t int_low_acc; uint32_t count; uint32_t next_step_time, step_pulse_ticks; struct gpio_out step_pin, dir_pin; @@ -57,9 +65,39 @@ enum { POSITION_BIAS=0x40000000 }; enum { SF_LAST_DIR=1<<0, SF_NEXT_DIR=1<<1, SF_INVERT_STEP=1<<2, SF_NEED_RESET=1<<3, - SF_SINGLE_SCHED=1<<4, SF_HAVE_ADD=1<<5 + SF_SINGLE_SCHED=1<<4, SF_HAVE_ADD=1<<5, SF_HIGH_PREC_STEP=1<<6 }; +// High-precision stepping interval functions +static inline void +add_interval(uint32_t* time, struct stepper *s) +{ + if (CONFIG_MACH_AVR) { + if (s->shift == 16) { + uint32_t interval = s->interval + s->int_low_acc; + *time += interval >> 16; + s->int_low_acc = interval & 0xFFFF; + } else if (s->shift == 8) { + uint32_t interval = s->interval + s->int_low_acc; + *time += interval >> 8; + s->int_low_acc = interval & 0xFF; + } else { + *time += s->interval; + } + } else { + uint32_t interval = s->interval + s->int_low_acc; + *time += interval >> s->shift; + s->int_low_acc = interval - ((interval >> s->shift) << s->shift); + } +} + +static inline void +inc_interval(struct stepper *s) +{ + s->interval += s->add; + s->add += s->add2; +} + // Setup a stepper for the next move in its queue static uint_fast8_t stepper_load_next(struct stepper *s) @@ -73,17 +111,28 @@ stepper_load_next(struct stepper *s) // Load next 'struct stepper_move' into 'struct stepper' struct move_node *mn = move_queue_pop(&s->mq); struct stepper_move *m = container_of(mn, struct stepper_move, node); + s->interval = m->interval; s->add = m->add; - s->interval = m->interval + m->add; + if (m->flags & SF_HIGH_PREC_STEP) { + s->add2 = m->add2; + s->shift = m->shift; + s->int_low_acc = m->int_low_acc; + s->flags = s->flags | SF_HIGH_PREC_STEP; + } else + s->flags = s->flags & ~SF_HIGH_PREC_STEP; if (HAVE_SINGLE_SCHEDULE && s->flags & SF_SINGLE_SCHED) { - s->time.waketime += m->interval; - if (HAVE_AVR_OPTIMIZATION) - s->flags = m->add ? s->flags|SF_HAVE_ADD : s->flags & ~SF_HAVE_ADD; + s->time.waketime += m->first_interval; + if (HAVE_AVR_OPTIMIZATION) { + if (m->flags & SF_HAVE_ADD) + s->flags = s->flags | SF_HAVE_ADD; + else + s->flags = s->flags & ~SF_HAVE_ADD; + } s->count = m->count; } else { // It is necessary to schedule unstep events and so there are // twice as many events. - s->next_step_time += m->interval; + s->next_step_time += m->first_interval; s->time.waketime = s->next_step_time; s->count = (uint32_t)m->count * 2; } @@ -108,14 +157,19 @@ stepper_event_edge(struct timer *t) uint32_t count = s->count - 1; if (likely(count)) { s->count = count; - s->time.waketime += s->interval; - s->interval += s->add; + if (s->flags & SF_HIGH_PREC_STEP) { + add_interval(&s->time.waketime, s); + inc_interval(s); + } else { + s->time.waketime += s->interval; + s->interval += s->add; + } return SF_RESCHEDULE; } return stepper_load_next(s); } -#define AVR_STEP_INSNS 40 // minimum instructions between step gpio pulses +#define AVR_STEP_INSNS 45 // minimum instructions between step gpio pulses // AVR optimized step function static uint_fast8_t @@ -126,10 +180,17 @@ stepper_event_avr(struct timer *t) uint16_t *pcount = (void*)&s->count, count = *pcount - 1; if (likely(count)) { *pcount = count; - s->time.waketime += s->interval; - gpio_out_toggle_noirq(s->step_pin); - if (s->flags & SF_HAVE_ADD) - s->interval += s->add; + if (s->flags & SF_HIGH_PREC_STEP) { + add_interval(&s->time.waketime, s); + gpio_out_toggle_noirq(s->step_pin); + if (s->flags & SF_HAVE_ADD) + inc_interval(s); + } else { + s->time.waketime += s->interval; + gpio_out_toggle_noirq(s->step_pin); + if (s->flags & SF_HAVE_ADD) + s->interval += s->add; + } return SF_RESCHEDULE; } uint_fast8_t ret = stepper_load_next(s); @@ -150,8 +211,13 @@ stepper_event_full(struct timer *t) // Schedule unstep event goto reschedule_min; if (likely(s->count)) { - s->next_step_time += s->interval; - s->interval += s->add; + if (s->flags & SF_HIGH_PREC_STEP) { + add_interval(&s->next_step_time, s); + inc_interval(s); + } else { + s->next_step_time += s->interval; + s->interval += s->add; + } if (unlikely(timer_is_before(s->next_step_time, min_next_time))) // The next step event is too close - push it back goto reschedule_min; @@ -222,12 +288,16 @@ command_queue_step(uint32_t *args) { struct stepper *s = stepper_oid_lookup(args[0]); struct stepper_move *m = move_alloc(); - m->interval = args[1]; m->count = args[2]; if (!m->count) shutdown("Invalid count parameter"); + m->first_interval = args[1]; m->add = args[3]; + m->interval = m->first_interval + m->add; m->flags = 0; + if (HAVE_AVR_OPTIMIZATION) { + if (m->add) m->flags |= SF_HAVE_ADD; + } irq_disable(); uint8_t flags = s->flags; @@ -251,6 +321,91 @@ command_queue_step(uint32_t *args) DECL_COMMAND(command_queue_step, "queue_step oid=%c interval=%u count=%hu add=%hi"); +void +command_queue_step_hp(uint32_t *args) +{ + struct stepper *s = stepper_oid_lookup(args[0]); + struct stepper_move *m = move_alloc(); + m->count = args[2]; + if (!m->count || m->count >= 0x8000) + shutdown("Invalid count parameter"); + uint32_t interval = args[1]; + int32_t add = args[3]; + int32_t add2 = args[4]; + int8_t shift = args[5]; + + if (shift <= 0) { + interval <<= -shift; + // Left shift of a negative int is an undefined behavior in C + add = add >= 0 ? add << -shift : -(-add << -shift); + add2 = add2 >= 0 ? add2 << -shift : -(-add2 << -shift); + m->interval = interval + add; + m->add = add + add2; + m->add2 = add2; + m->int_low_acc = 0; + m->first_interval = interval; + m->shift = 0; + } else if (CONFIG_MACH_AVR) { + uint_fast8_t extra_shift = shift > 8 ? 16-shift : 8-shift; + m->shift = shift > 8 ? 16 : 8; + interval <<= extra_shift; + add = add >= 0 ? add << extra_shift : -(-add << extra_shift); + add2 = add2 >= 0 ? add2 << extra_shift : -(-add2 << extra_shift); + m->interval = interval + add; + m->add = add + add2; + m->add2 = add2; + if (m->shift == 16) { + m->int_low_acc = 1 << 15; + interval += m->int_low_acc; + m->first_interval = interval >> 16; + m->int_low_acc = interval & 0xFFFF; + } else { // m->shift == 8 + m->int_low_acc = 1 << 7; + interval += m->int_low_acc; + m->first_interval = interval >> 8; + m->int_low_acc = interval & 0xFF; + } + } else { + m->interval = interval + add; + m->add = add + add2; + m->add2 = add2; + m->int_low_acc = 1 << (shift - 1); + interval += m->int_low_acc; + m->first_interval = interval >> shift; + m->int_low_acc = interval - ((interval >> shift) << shift); + m->shift = shift; + } + + m->flags = SF_HIGH_PREC_STEP; + if (HAVE_AVR_OPTIMIZATION) { + if (m->add || m->add2) { + m->flags |= SF_HAVE_ADD; + } + } + + irq_disable(); + uint8_t flags = s->flags; + if (!!(flags & SF_LAST_DIR) != !!(flags & SF_NEXT_DIR)) { + flags ^= SF_LAST_DIR; + m->flags |= MF_DIR; + } + if (s->count) { + s->flags = flags; + move_queue_push(&m->node, &s->mq); + } else if (flags & SF_NEED_RESET) { + move_free(m); + } else { + s->flags = flags; + move_queue_push(&m->node, &s->mq); + stepper_load_next(s); + sched_add_timer(&s->time); + } + irq_enable(); +} +DECL_COMMAND(command_queue_step_hp, + "queue_step_hp oid=%c interval=%u count=%hu add=%hi " + "add2=%hi shift=%hi"); + // Set the direction of the next queued step void command_set_next_step_dir(uint32_t *args) From b8c0523878b0f6ec5fa69076bef65addad6cd5bb Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sat, 15 Apr 2023 23:35:18 +0200 Subject: [PATCH 17/35] stepper: Optionally enable new stepcompress protocol in MCU firmware Signed-off-by: Dmitry Butyugin --- klippy/stepper.py | 29 +++++++----- src/Kconfig | 6 +++ src/avr/Kconfig | 8 ++++ src/stepper.c | 109 +++++++++++++++++++++++++--------------------- 4 files changed, 90 insertions(+), 62 deletions(-) diff --git a/klippy/stepper.py b/klippy/stepper.py index fd81fb5b5..a2f263e9d 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -3,9 +3,8 @@ # Copyright (C) 2016-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import collections -import math -import chelper +import math, logging, collections +import chelper, msgproto class error(Exception): @@ -21,11 +20,11 @@ class error(Exception): # Interface to low-level mcu and chelper code class MCU_stepper: - def __init__(self, name, high_precision_steps, step_pin_params, + def __init__(self, name, high_precision_stepcompr, step_pin_params, dir_pin_params, rotation_dist, steps_per_rotation, step_pulse_duration=None, units_in_radians=False): self._name = name - self._high_precision_steps = high_precision_steps + self._high_precision_stepcompr = high_precision_stepcompr self._rotation_dist = rotation_dist self._steps_per_rotation = steps_per_rotation self._step_pulse_duration = step_pulse_duration @@ -47,7 +46,7 @@ def __init__(self, name, high_precision_steps, step_pin_params, self._reset_cmd_tag = self._get_position_cmd = None self._active_callbacks = [] ffi_main, ffi_lib = chelper.get_ffi() - if high_precision_steps: + if high_precision_stepcompr: self._stepqueue = ffi_main.gc(ffi_lib.stepcompress_hp_alloc(oid), ffi_lib.stepcompress_free) else: @@ -116,10 +115,15 @@ def _build_config(self): invert_step, step_pulse_ticks)) self._mcu.add_config_cmd("reset_step_clock oid=%d clock=0" % (self._oid,), on_restart=True) - if self._high_precision_steps: - step_cmd_tag = self._mcu.lookup_command( - "queue_step_hp oid=%c interval=%u count=%hu " - "add=%hi add2=%hi shift=%hi").get_command_tag() + if self._high_precision_stepcompr: + try: + step_cmd_tag = self._mcu.lookup_command( + "queue_step_hp oid=%c interval=%u count=%hu " + "add=%hi add2=%hi shift=%hi").get_command_tag() + except msgproto.MessageParser.error as e: + cerror = self._mcu.get_printer().config_error + raise cerror(str(e) + ", maybe MCU firmware was compiled " + + "without high precision stepping support?") else: step_cmd_tag = self._mcu.lookup_command( "queue_step oid=%c interval=%u count=%hu " @@ -310,8 +314,9 @@ def PrinterStepper(config, units_in_radians=False): config, units_in_radians, True) step_pulse_duration = config.getfloat('step_pulse_duration', None, minval=0., maxval=.001) - high_precision_steps = config.getboolean('high_precision_steps', False) - mcu_stepper = MCU_stepper(name, high_precision_steps, step_pin_params, + high_precision_stepcompr = config.getboolean('high_precision_step_compress', + False) + mcu_stepper = MCU_stepper(name, high_precision_stepcompr, step_pin_params, dir_pin_params, rotation_dist, steps_per_rotation, step_pulse_duration, units_in_radians) # Register with helper modules diff --git a/src/Kconfig b/src/Kconfig index 3c49a73f9..502e60fd6 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -152,6 +152,12 @@ config INITIAL_PINS pins will be set to output high - preface a pin with a '!' character to set that pin to output low. +config HIGH_PREC_STEP + depends on LOW_LEVEL_OPTIONS && \ + FLASH_SIZE >= 0x8000 && (!MACH_AVR || FLASH_SIZE >= 0x10000) + bool "High-precision stepping support (slow)" + default n + # The HAVE_x options allow boards to disable support for some commands # if the hardware does not support the feature. config HAVE_GPIO diff --git a/src/avr/Kconfig b/src/avr/Kconfig index ac3ffa2ab..8aa47d374 100644 --- a/src/avr/Kconfig +++ b/src/avr/Kconfig @@ -77,6 +77,14 @@ config CLOCK_FREQ default 20000000 if AVR_FREQ_20000000 default 16000000 +config FLASH_SIZE + hex + default 0x4000 if MACH_atmega168 + default 0x8000 if MACH_atmega328 || MACH_atmega328p || MACH_atmega32u4 + default 0x10000 if MACH_atmega644p || MACH_at90usb646 + default 0x20000 if MACH_atmega1280 || MACH_atmega1284p || MACH_at90usb1286 + default 0x40000 if MACH_atmega2560 + config CLEAR_PRESCALER bool "Manually clear the CPU prescaler field at startup" if LOW_LEVEL_OPTIONS depends on MACH_at90usb1286 || MACH_at90usb646 || MACH_atmega32u4 diff --git a/src/stepper.c b/src/stepper.c index ae09a59cd..b57b01884 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -31,13 +31,17 @@ struct stepper_move { struct move_node node; - uint32_t first_interval; uint32_t interval; +#if CONFIG_HIGH_PREC_STEP // Extra fields for high precision stepping + uint32_t next_interval; int32_t add; int32_t add2; uint_fast8_t shift; uint16_t int_low_acc; +#else + int16_t add; +#endif uint16_t count; uint8_t flags; }; @@ -47,10 +51,14 @@ enum { MF_DIR=1<<0 }; struct stepper { struct timer time; uint32_t interval; +#if CONFIG_HIGH_PREC_STEP int32_t add; int32_t add2; uint_fast8_t shift; uint16_t int_low_acc; +#else + int16_t add; +#endif uint32_t count; uint32_t next_step_time, step_pulse_ticks; struct gpio_out step_pin, dir_pin; @@ -72,6 +80,7 @@ enum { static inline void add_interval(uint32_t* time, struct stepper *s) { +#if CONFIG_HIGH_PREC_STEP if (CONFIG_MACH_AVR) { if (s->shift == 16) { uint32_t interval = s->interval + s->int_low_acc; @@ -89,13 +98,16 @@ add_interval(uint32_t* time, struct stepper *s) *time += interval >> s->shift; s->int_low_acc = interval - ((interval >> s->shift) << s->shift); } +#endif } static inline void inc_interval(struct stepper *s) { +#if CONFIG_HIGH_PREC_STEP s->interval += s->add; s->add += s->add2; +#endif } // Setup a stepper for the next move in its queue @@ -111,8 +123,9 @@ stepper_load_next(struct stepper *s) // Load next 'struct stepper_move' into 'struct stepper' struct move_node *mn = move_queue_pop(&s->mq); struct stepper_move *m = container_of(mn, struct stepper_move, node); - s->interval = m->interval; s->add = m->add; +#if CONFIG_HIGH_PREC_STEP + s->interval = m->next_interval; if (m->flags & SF_HIGH_PREC_STEP) { s->add2 = m->add2; s->shift = m->shift; @@ -120,8 +133,11 @@ stepper_load_next(struct stepper *s) s->flags = s->flags | SF_HIGH_PREC_STEP; } else s->flags = s->flags & ~SF_HIGH_PREC_STEP; +#else + s->interval = m->interval + m->add; +#endif if (HAVE_SINGLE_SCHEDULE && s->flags & SF_SINGLE_SCHED) { - s->time.waketime += m->first_interval; + s->time.waketime += m->interval; if (HAVE_AVR_OPTIMIZATION) { if (m->flags & SF_HAVE_ADD) s->flags = s->flags | SF_HAVE_ADD; @@ -132,7 +148,7 @@ stepper_load_next(struct stepper *s) } else { // It is necessary to schedule unstep events and so there are // twice as many events. - s->next_step_time += m->first_interval; + s->next_step_time += m->interval; s->time.waketime = s->next_step_time; s->count = (uint32_t)m->count * 2; } @@ -157,7 +173,7 @@ stepper_event_edge(struct timer *t) uint32_t count = s->count - 1; if (likely(count)) { s->count = count; - if (s->flags & SF_HIGH_PREC_STEP) { + if (CONFIG_HIGH_PREC_STEP && (s->flags & SF_HIGH_PREC_STEP)) { add_interval(&s->time.waketime, s); inc_interval(s); } else { @@ -180,15 +196,15 @@ stepper_event_avr(struct timer *t) uint16_t *pcount = (void*)&s->count, count = *pcount - 1; if (likely(count)) { *pcount = count; - if (s->flags & SF_HIGH_PREC_STEP) { + if (CONFIG_HIGH_PREC_STEP && (s->flags & SF_HIGH_PREC_STEP)) { add_interval(&s->time.waketime, s); gpio_out_toggle_noirq(s->step_pin); - if (s->flags & SF_HAVE_ADD) + if (HAVE_AVR_OPTIMIZATION && (s->flags & SF_HAVE_ADD)) inc_interval(s); } else { s->time.waketime += s->interval; gpio_out_toggle_noirq(s->step_pin); - if (s->flags & SF_HAVE_ADD) + if (HAVE_AVR_OPTIMIZATION && (s->flags & SF_HAVE_ADD)) s->interval += s->add; } return SF_RESCHEDULE; @@ -211,7 +227,7 @@ stepper_event_full(struct timer *t) // Schedule unstep event goto reschedule_min; if (likely(s->count)) { - if (s->flags & SF_HIGH_PREC_STEP) { + if (CONFIG_HIGH_PREC_STEP && (s->flags & SF_HIGH_PREC_STEP)) { add_interval(&s->next_step_time, s); inc_interval(s); } else { @@ -282,23 +298,9 @@ stepper_oid_lookup(uint8_t oid) return oid_lookup(oid, command_config_stepper); } -// Schedule a set of steps with a given timing -void -command_queue_step(uint32_t *args) +static void +enqueue_move(struct stepper *s, struct stepper_move *m) { - struct stepper *s = stepper_oid_lookup(args[0]); - struct stepper_move *m = move_alloc(); - m->count = args[2]; - if (!m->count) - shutdown("Invalid count parameter"); - m->first_interval = args[1]; - m->add = args[3]; - m->interval = m->first_interval + m->add; - m->flags = 0; - if (HAVE_AVR_OPTIMIZATION) { - if (m->add) m->flags |= SF_HAVE_ADD; - } - irq_disable(); uint8_t flags = s->flags; if (!!(flags & SF_LAST_DIR) != !!(flags & SF_NEXT_DIR)) { @@ -318,9 +320,32 @@ command_queue_step(uint32_t *args) } irq_enable(); } + +// Schedule a set of steps with a given timing +void +command_queue_step(uint32_t *args) +{ + struct stepper *s = stepper_oid_lookup(args[0]); + struct stepper_move *m = move_alloc(); + m->interval = args[1]; + m->count = args[2]; + if (!m->count) + shutdown("Invalid count parameter"); + m->add = args[3]; +#if CONFIG_HIGH_PREC_STEP + m->next_interval = m->interval + m->add; +#endif + m->flags = 0; + if (HAVE_AVR_OPTIMIZATION) { + if (m->add) m->flags |= SF_HAVE_ADD; + } + + enqueue_move(s, m); +} DECL_COMMAND(command_queue_step, "queue_step oid=%c interval=%u count=%hu add=%hi"); +#if CONFIG_HIGH_PREC_STEP void command_queue_step_hp(uint32_t *args) { @@ -339,11 +364,11 @@ command_queue_step_hp(uint32_t *args) // Left shift of a negative int is an undefined behavior in C add = add >= 0 ? add << -shift : -(-add << -shift); add2 = add2 >= 0 ? add2 << -shift : -(-add2 << -shift); - m->interval = interval + add; + m->next_interval = interval + add; m->add = add + add2; m->add2 = add2; m->int_low_acc = 0; - m->first_interval = interval; + m->interval = interval; m->shift = 0; } else if (CONFIG_MACH_AVR) { uint_fast8_t extra_shift = shift > 8 ? 16-shift : 8-shift; @@ -351,27 +376,27 @@ command_queue_step_hp(uint32_t *args) interval <<= extra_shift; add = add >= 0 ? add << extra_shift : -(-add << extra_shift); add2 = add2 >= 0 ? add2 << extra_shift : -(-add2 << extra_shift); - m->interval = interval + add; + m->next_interval = interval + add; m->add = add + add2; m->add2 = add2; if (m->shift == 16) { m->int_low_acc = 1 << 15; interval += m->int_low_acc; - m->first_interval = interval >> 16; + m->interval = interval >> 16; m->int_low_acc = interval & 0xFFFF; } else { // m->shift == 8 m->int_low_acc = 1 << 7; interval += m->int_low_acc; - m->first_interval = interval >> 8; + m->interval = interval >> 8; m->int_low_acc = interval & 0xFF; } } else { - m->interval = interval + add; + m->next_interval = interval + add; m->add = add + add2; m->add2 = add2; m->int_low_acc = 1 << (shift - 1); interval += m->int_low_acc; - m->first_interval = interval >> shift; + m->interval = interval >> shift; m->int_low_acc = interval - ((interval >> shift) << shift); m->shift = shift; } @@ -383,28 +408,12 @@ command_queue_step_hp(uint32_t *args) } } - irq_disable(); - uint8_t flags = s->flags; - if (!!(flags & SF_LAST_DIR) != !!(flags & SF_NEXT_DIR)) { - flags ^= SF_LAST_DIR; - m->flags |= MF_DIR; - } - if (s->count) { - s->flags = flags; - move_queue_push(&m->node, &s->mq); - } else if (flags & SF_NEED_RESET) { - move_free(m); - } else { - s->flags = flags; - move_queue_push(&m->node, &s->mq); - stepper_load_next(s); - sched_add_timer(&s->time); - } - irq_enable(); + enqueue_move(s, m); } DECL_COMMAND(command_queue_step_hp, "queue_step_hp oid=%c interval=%u count=%hu add=%hi " "add2=%hi shift=%hi"); +#endif // Set the direction of the next queued step void From ad33469ff6e0aa2a061fb00fe561d36be62f3548 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 23 Apr 2023 17:34:43 +0200 Subject: [PATCH 18/35] input_shaper: Added custom input shapers support Signed-off-by: Dmitry Butyugin --- klippy/extras/input_shaper.py | 215 ++++++++++++++++++++++++---------- 1 file changed, 152 insertions(+), 63 deletions(-) diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 157bbba84..a4362c988 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -8,42 +8,48 @@ import chelper from . import shaper_defs +def parse_float_list(list_str): + def parse_str(s): + res = [] + for line in s.split('\n'): + for coeff in line.split(','): + res.append(float(coeff.strip())) + return res + try: + return parse_str(list_str) + except: + return None -class InputShaperParams: - def __init__(self, axis, config): +class TypedInputShaperParams: + shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS} + def __init__(self, axis, shaper_type, config): self.axis = axis - self.shapers = {s.name: s.init_func for s in shaper_defs.INPUT_SHAPERS} - shaper_type = config.get("shaper_type", "mzv") - self.shaper_type = config.get("shaper_type_" + axis, shaper_type) - if self.shaper_type not in self.shapers: - raise config.error( - "Unsupported shaper type: %s" % (self.shaper_type,) - ) - self.damping_ratio = config.getfloat( - "damping_ratio_" + axis, - shaper_defs.DEFAULT_DAMPING_RATIO, - minval=0.0, - maxval=1.0, - ) - self.shaper_freq = config.getfloat( - "shaper_freq_" + axis, 0.0, minval=0.0 - ) - - def update(self, gcmd): + self.shaper_type = shaper_type + self.damping_ratio = shaper_defs.DEFAULT_DAMPING_RATIO + self.shaper_freq = 0. + if config is not None: + if shaper_type not in self.shapers: + raise config.error( + 'Unsupported shaper type: %s' % (shaper_type,)) + self.damping_ratio = config.getfloat('damping_ratio_' + axis, + self.damping_ratio, + minval=0., maxval=1.) + self.shaper_freq = config.getfloat('shaper_freq_' + axis, + self.shaper_freq, minval=0.) + def get_type(self): + return self.shaper_type + def get_axis(self): + return self.axis + def update(self, shaper_type, gcmd): + if shaper_type not in self.shapers: + raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) axis = self.axis.upper() - self.damping_ratio = gcmd.get_float( - "DAMPING_RATIO_" + axis, self.damping_ratio, minval=0.0, maxval=1.0 - ) - self.shaper_freq = gcmd.get_float( - "SHAPER_FREQ_" + axis, self.shaper_freq, minval=0.0 - ) - shaper_type = gcmd.get("SHAPER_TYPE", None) - if shaper_type is None: - shaper_type = gcmd.get("SHAPER_TYPE_" + axis, self.shaper_type) - if shaper_type.lower() not in self.shapers: - raise gcmd.error("Unsupported shaper type: %s" % (shaper_type,)) - self.shaper_type = shaper_type.lower() - + self.damping_ratio = gcmd.get_float('DAMPING_RATIO_' + axis, + self.damping_ratio, + minval=0., maxval=1.) + self.shaper_freq = gcmd.get_float('SHAPER_FREQ_' + axis, + self.shaper_freq, minval=0.) + self.shaper_type = shaper_type def get_shaper(self): if not self.shaper_freq: A, T = shaper_defs.get_none_shaper() @@ -63,46 +69,99 @@ def get_status(self): ) -class AxisInputShaper: +class CustomInputShaperParams: + SHAPER_TYPE = 'custom' def __init__(self, axis, config): self.axis = axis - self.params = InputShaperParams(axis, config) - self.n, self.A, self.T = self.params.get_shaper() - self.saved = None - - def get_name(self): - return 'shaper_' + self.axis + self.n, self.A, self.T = 0, [], [] + if config is not None: + shaper_a_str = config.get('shaper_a_' + axis) + shaper_t_str = config.get('shaper_t_' + axis) + self.n, self.A, self.T = self._parse_custom_shaper( + shaper_a_str, shaper_t_str, config.error) + def get_type(self): + return self.SHAPER_TYPE def get_axis(self): return self.axis + def update(self, shaper_type, gcmd): + if shaper_type != self.SHAPER_TYPE: + raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) + axis = self.axis.upper() + shaper_a_str = gcmd.get('SHAPER_A_' + axis, None) + shaper_t_str = gcmd.get('SHAPER_T_' + axis, None) + if (shaper_a_str is None) != (shaper_t_str is None): + raise gcmd.error("Both SHAPER_A_%s and SHAPER_T_%s parameters" + " must be provided" % (axis, axis)) + if shaper_a_str is not None: + self.n, self.A, self.T = self._parse_custom_shaper( + shaper_a_str, shaper_t_str, gcmd.error) + def _parse_custom_shaper(self, custom_a_str, custom_t_str, parse_error): + A = parse_float_list(custom_a_str) + if A is None: + raise parse_error("Invalid shaper A string: '%s'" % (custom_a_str,)) + if min([abs(a) for a in A]) < 0.001: + raise parse_error("All shaper A coefficients must be non-zero") + if sum(A) < 0.001: + raise parse_error( + "Shaper A parameter must sum up to a positive number") + T = parse_float_list(custom_t_str) + if T is None: + raise parse_error("Invalid shaper T string: '%s'" % (custom_t_str,)) + if T != sorted(T): + raise parse_error("Shaper T parameter is not ordered: %s" % (T,)) + if len(A) != len(T): + raise parse_error( + "Shaper A and T parameters must have the same length:" + " %d vs %d" % (len(A), len(T),)) + dur = T[-1] - T[0] + if len(T) > 1 and dur < 0.001: + raise parse_error("Shaper duration is too small (%.6f sec)" + % (dur,)) + if dur > 0.2: + raise parse_error("Shaper duration is too large (%.6f sec)" + % (dur,)) + return len(A), A, T def get_shaper(self): return self.n, self.A, self.T + def get_status(self): + return collections.OrderedDict([ + ('shaper_type', self.SHAPER_TYPE), + ('shaper_a', ','.join(['%.6f' % (a,) for a in self.A])), + ('shaper_t', ','.join(['%.6f' % (t,) for t in self.T]))]) - def update(self, gcmd): - self.params.update(gcmd) - old_n, old_A, old_T = self.n, self.A, self.T +class AxisInputShaper: + def __init__(self, params): + self.params = params + self.n, self.A, self.T = params.get_shaper() + self.saved = None + def get_name(self): + return 'shaper_' + self.get_axis() + def get_type(self): + return self.params.get_type() + def get_axis(self): + return self.params.get_axis() + def update(self, shaper_type, gcmd): + self.params.update(shaper_type, gcmd) self.n, self.A, self.T = self.params.get_shaper() def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() - success = ( - ffi_lib.input_shaper_set_shaper_params( - sk, self.axis.encode(), self.n, self.A, self.T - ) - == 0 - ) + axis = self.get_axis().encode() + success = ffi_lib.input_shaper_set_shaper_params( + sk, axis, self.n, self.A, self.T) == 0 if not success: self.disable_shaping() ffi_lib.input_shaper_set_shaper_params( - sk, self.axis.encode(), self.n, self.A, self.T - ) + sk, axis, self.n, self.A, self.T) return success def update_extruder_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() + axis = self.get_axis().encode() success = ffi_lib.extruder_set_shaper_params( - sk, self.axis.encode(), self.n, self.A, self.T) == 0 + sk, axis, self.n, self.A, self.T) == 0 if not success: self.disable_shaping() ffi_lib.extruder_set_shaper_params( - sk, self.axis.encode(), self.n, self.A, self.T) + sk, axis, self.n, self.A, self.T) return success def disable_shaping(self): was_enabled = False @@ -120,14 +179,43 @@ def enable_shaping(self): self.saved = None return True def report(self, gcmd): - info = " ".join( - [ - "%s_%s:%s" % (key, self.axis, value) - for (key, value) in self.params.get_status().items() - ] - ) + info = ' '.join(["%s_%s:%s" % (key, self.get_axis(), value) + for (key, value) in self.params.get_status().items()]) gcmd.respond_info(info) +class ShaperFactory: + def __init__(self): + pass + def _create_shaper(self, axis, type_name, config=None): + if type_name == CustomInputShaperParams.SHAPER_TYPE: + return AxisInputShaper(CustomInputShaperParams(axis, config)) + if type_name in TypedInputShaperParams.shapers: + return AxisInputShaper( + TypedInputShaperParams(axis, type_name, config)) + return None + def create_shaper(self, axis, config): + shaper_type = config.get('shaper_type', 'mzv') + shaper_type = config.get('shaper_type_' + axis, shaper_type).lower() + shaper = self._create_shaper(axis, shaper_type, config) + if shaper is None: + raise config.error("Unsupported shaper type '%s'" % (shaper_type,)) + return shaper + def update_shaper(self, shaper, gcmd): + shaper_type = gcmd.get('SHAPER_TYPE', None) + if shaper_type is None: + shaper_type = gcmd.get('SHAPER_TYPE_' + shaper.get_axis().upper(), + shaper.get_type()) + shaper_type = shaper_type.lower() + try: + shaper.update(shaper_type, gcmd) + return shaper + except gcmd.error: + pass + shaper = self._create_shaper(shaper.get_axis(), shaper_type) + if shaper is None: + raise gcmd.error("Unsupported shaper type '%s'" % (shaper_type,)) + shaper.update(shaper_type, gcmd) + return shaper class InputShaper: def __init__(self, config): @@ -136,8 +224,9 @@ def __init__(self, config): self.toolhead = None self.extruders = [] self.config_extruder_names = config.getlist('enabled_extruders', []) - self.shapers = [AxisInputShaper('x', config), - AxisInputShaper('y', config)] + self.shaper_factory = ShaperFactory() + self.shapers = [self.shaper_factory.create_shaper('x', config), + self.shaper_factory.create_shaper('y', config)] self.input_shaper_stepper_kinematics = [] self.orig_stepper_kinematics = [] # Register gcode commands @@ -229,8 +318,8 @@ def enable_shaping(self): def cmd_SET_INPUT_SHAPER(self, gcmd): if gcmd.get_command_parameters(): - for shaper in self.shapers: - shaper.update(gcmd) + self.shapers = [self.shaper_factory.update_shaper(shaper, gcmd) + for shaper in self.shapers] self._update_input_shaping() for shaper in self.shapers: shaper.report(gcmd) From eebf356ed17df677ea5b4bf3206dc490d3d2c6ed Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 23 Apr 2023 18:22:43 +0200 Subject: [PATCH 19/35] input_shaper: Added support of smooth input shapers Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 11 ++- klippy/chelper/integrate.c | 97 ++++++++++++++++++++++ klippy/chelper/integrate.h | 15 ++++ klippy/chelper/kin_extruder.c | 146 ++++++++++++++------------------- klippy/chelper/kin_shaper.c | 149 ++++++++++++++++++++++++++++------ klippy/chelper/kin_shaper.h | 4 +- klippy/chelper/trapq.c | 4 +- klippy/chelper/trapq.h | 4 +- klippy/extras/input_shaper.py | 107 ++++++++++++++++++++++++ klippy/kinematics/extruder.py | 86 +++++++++++++++----- 10 files changed, 485 insertions(+), 138 deletions(-) create mode 100644 klippy/chelper/integrate.c create mode 100644 klippy/chelper/integrate.h diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 50cf1a641..8e795ce86 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -23,12 +23,12 @@ 'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', 'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', - 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', + 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'integrate.c', ] DEST_LIB = "c_helper.so" OTHER_FILES = [ 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', - 'trapq.h', 'pollreactor.h', 'msgblock.h', 'kin_shaper.h' + 'trapq.h', 'pollreactor.h', 'msgblock.h', 'kin_shaper.h', 'integrate.h', ] defs_stepcompress = """ @@ -142,8 +142,7 @@ defs_kin_extruder = """ struct stepper_kinematics *extruder_stepper_alloc(void); void extruder_set_pressure_advance(struct stepper_kinematics *sk - , int n_params, double params[] - , double smooth_time, double time_offset); + , int n_params, double params[], double time_offset); struct pressure_advance_params; double pressure_advance_linear_model_func(double position , double pa_velocity, struct pressure_advance_params *pa_params); @@ -155,6 +154,8 @@ , double (*func)(double, double, struct pressure_advance_params *)); int extruder_set_shaper_params(struct stepper_kinematics *sk, char axis , int n, double a[], double t[]); + int extruder_set_smoothing_params(struct stepper_kinematics *sk, char axis + , int n, double a[], double t_sm); double extruder_get_step_gen_window(struct stepper_kinematics *sk); """ @@ -162,6 +163,8 @@ double input_shaper_get_step_gen_window(struct stepper_kinematics *sk); int input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis , int n, double a[], double t[]); + int input_shaper_set_smoother_params(struct stepper_kinematics *sk + , char axis, int n, double a[], double t_sm); int input_shaper_set_sk(struct stepper_kinematics *sk , struct stepper_kinematics *orig_sk); struct stepper_kinematics * input_shaper_alloc(void); diff --git a/klippy/chelper/integrate.c b/klippy/chelper/integrate.c new file mode 100644 index 000000000..6f5845337 --- /dev/null +++ b/klippy/chelper/integrate.c @@ -0,0 +1,97 @@ +// Helpers to integrate the smoothing weight function +// +// Copyright (C) 2019-2020 Kevin O'Connor +// Copyright (C) 2020-2023 Dmitry Butyugin +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "compiler.h" +#include "integrate.h" + +#include + +/**************************************************************** + * Generic smoother integration + ****************************************************************/ + +static double coeffs[] = { + 1./1., 1./2., 1./3., 1./4., 1./5., 1./6., 1./7., 1./8., 1./9., 1./10., + 1./11., 1./12., 1./13., 1./14., 1./15., 1./16., 1./17., 1./18., 1./19., +}; + +inline static double +integrate_nth_order(int n, const double c[], double start, double end + , double a0, double a1, double a2) +{ + double u = start + end; + double v = u * .5; + double end_n = end; + // First 2 iterations with i == 0 and 1 + double sum0 = c[0] + c[1] * v, sum1 = c[0] * v, sum2 = 0.; + int i; + for (i = 2; i < n; ++i) { + end_n *= end; + u = u * start + end_n; + v = u * coeffs[i]; + sum2 += c[i-2] * v; + sum1 += c[i-1] * v; + sum0 += c[i] * v; + } + // Remainging 2 iterations with i == n and n+1 + end_n *= end; + u = u * start + end_n; + v = u * coeffs[n]; + sum2 += c[n-2] * v; + sum1 += c[n-1] * v; + + end_n *= end; + u = u * start + end_n; + v = u * coeffs[n+1]; + sum2 += c[n-1] * v; + + double delta_t = end - start; + double avg_val = a0 * sum0 + a1 * sum1 + a2 * sum2; + return avg_val * delta_t; +} + +// Integrate (pos + start_v*t + half_accel*t^2) with smoothing weight function +// w(t0 - t) over the range [start; end] +inline double +integrate_weighted(const struct smoother *sm, double pos + , double start_v, double half_accel + , double start, double end, double t0) +{ + // Substitute the integration variable tnew = t0 - t to simplify integrals + pos += (half_accel * t0 + start_v) * t0; + start_v = -(start_v + 2. * half_accel * t0); + return integrate_nth_order(sm->n, sm->c, t0 - end, t0 - start + , pos, start_v, half_accel); +} + +/**************************************************************** + * Smoother initialization + ****************************************************************/ + +int +init_smoother(int n, const double a[], double t_sm, struct smoother* sm) +{ + if (n < 0 || n > ARRAY_SIZE(sm->c)) + return -1; + memset(sm->c, 0, sizeof(sm->c)); + sm->n = n; + sm->hst = 0.5 * t_sm; + if (!t_sm) return 0; + double inv_t_sm = 1. / t_sm; + double inv_t_sm_n = inv_t_sm; + int i; + for (i = 0; i < n; ++i) { + sm->c[i] = a[i] * inv_t_sm_n; + inv_t_sm_n *= inv_t_sm; + } + double inv_norm = 1. / integrate_nth_order(n, sm->c, -sm->hst, sm->hst + , 1., 0., 0.); + for (i = 0; i < n; ++i) + sm->c[i] *= inv_norm; + sm->t_offs = integrate_nth_order(n, sm->c, -sm->hst, sm->hst, 0., 1., 0.); + return 0; +} diff --git a/klippy/chelper/integrate.h b/klippy/chelper/integrate.h new file mode 100644 index 000000000..22856979a --- /dev/null +++ b/klippy/chelper/integrate.h @@ -0,0 +1,15 @@ +#ifndef INTEGRATE_H +#define INTEGRATE_H + +struct smoother { + double c[12]; + double hst, t_offs; + int n; +}; + +int init_smoother(int n, const double a[], double t_sm, struct smoother* sm); +double integrate_weighted(const struct smoother *sm, + double pos, double start_v, double half_accel, + double start, double end, double t0); + +#endif // integrate.h diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index f2156128c..62c27fddb 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -10,6 +10,7 @@ #include // memset #include "compiler.h" // __visible #include "itersolve.h" // struct stepper_kinematics +#include "integrate.h" // struct smoother #include "kin_shaper.h" // struct shaper_pulses #include "pyhelper.h" // errorf #include "trapq.h" // move_get_distance @@ -23,33 +24,15 @@ // + pressure_advance * nominal_velocity(t)) // Which is then "smoothed" using a weighted average: // smooth_position(t) = ( -// definitive_integral(pa_position(x) * (smooth_time/2 - abs(t-x)) * dx, +// definitive_integral(pa_position(x + t_offs) * smoother(t-x) * dx, // from=t-smooth_time/2, to=t+smooth_time/2) // / ((smooth_time/2)**2)) -// Calculate the definitive integral of time weighted position: -// weighted_position(t) = t * (base + t * (start_v + t * half_accel)) -// - time_offset * (base + t * (start_v + t * half_accel)) -inline static double -extruder_integrate_weighted(double base, double start_v, double half_accel - , double start, double end, double time_offset) -{ - double delta_t = end - start; - double end2 = end * end; - double start2 = start * start; - double c1 = .5 * (end + start); - double c2 = 1./3. * (end2 + end * start + start2); - double c3 = .5 * c1 * (end2 + start2); - double avg_val = base * (c1 - time_offset) - + start_v * (c2 - c1 * time_offset) - + half_accel * (c3 - c2 * time_offset); - return delta_t * avg_val; -} - // Calculate the definitive integral of extruder for a given move static void -pa_move_integrate(struct move *m, int axis - , double base, double start, double end, double time_offset +pa_move_integrate(const struct move *m, int axis + , double base, double start, double end + , double t0, const struct smoother *sm , double *pos_integral, double *pa_velocity_integral) { if (start < 0.) @@ -62,21 +45,21 @@ pa_move_integrate(struct move *m, int axis double start_v = m->start_v * axis_r; double ha = m->half_accel * axis_r; // Calculate definitive integral - *pos_integral = extruder_integrate_weighted( - base, start_v, ha, start, end, time_offset); - if (!can_pressure_advance) { - *pa_velocity_integral = 0.; - } else { - *pa_velocity_integral = extruder_integrate_weighted( - start_v, 2. * ha, 0., start, end, time_offset); + *pos_integral += integrate_weighted( + sm, base, start_v, ha, start, end, t0); + if (can_pressure_advance) { + *pa_velocity_integral += integrate_weighted( + sm, start_v, 2. * ha, 0., start, end, t0); } } // Calculate the definitive integral of the extruder over a range of moves static void -pa_range_integrate(struct move *m, int axis, double move_time, double hst +pa_range_integrate(const struct move *m, int axis, double move_time + , const struct smoother *sm , double *pos_integral, double *pa_velocity_integral) { + move_time += sm->t_offs; while (unlikely(move_time < 0.)) { m = list_prev_entry(m, node); move_time += m->move_t; @@ -86,45 +69,36 @@ pa_range_integrate(struct move *m, int axis, double move_time, double hst m = list_next_entry(m, node); } // Calculate integral for the current move - *pos_integral = *pa_velocity_integral = 0.; - double m_pos_int, m_pa_vel_int; - double start = move_time - hst, end = move_time + hst; + double start = move_time - sm->hst, end = move_time + sm->hst; double start_base = m->start_pos.axis[axis - 'x']; - pa_move_integrate(m, axis, 0., start, move_time, start, - &m_pos_int, &m_pa_vel_int); - *pos_integral += m_pos_int; - *pa_velocity_integral += m_pa_vel_int; - pa_move_integrate(m, axis, 0., move_time, end, end, - &m_pos_int, &m_pa_vel_int); - *pos_integral -= m_pos_int; - *pa_velocity_integral -= m_pa_vel_int; + *pos_integral = *pa_velocity_integral = 0.; + pa_move_integrate(m, axis, 0., start, end, move_time, + sm, pos_integral, pa_velocity_integral); // Integrate over previous moves - struct move *prev = m; + const struct move *prev = m; while (unlikely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; double base = prev->start_pos.axis[axis - 'x'] - start_base; - pa_move_integrate(prev, axis, base, start, prev->move_t, start, - &m_pos_int, &m_pa_vel_int); - *pos_integral += m_pos_int; - *pa_velocity_integral += m_pa_vel_int; + pa_move_integrate(prev, axis, base, start, prev->move_t, + start + sm->hst, sm, pos_integral, + pa_velocity_integral); } // Integrate over future moves while (unlikely(end > m->move_t)) { end -= m->move_t; m = list_next_entry(m, node); double base = m->start_pos.axis[axis - 'x'] - start_base; - pa_move_integrate(m, axis, base, 0., end, end, - &m_pos_int, &m_pa_vel_int); - *pos_integral -= m_pos_int; - *pa_velocity_integral -= m_pa_vel_int; + pa_move_integrate(m, axis, base, 0., end, end - sm->hst, + sm, pos_integral, pa_velocity_integral); } - *pos_integral += start_base * hst * hst; + *pos_integral += start_base; } static void -shaper_pa_range_integrate(struct move *m, int axis, double move_time - , double hst, struct shaper_pulses *sp +shaper_pa_range_integrate(const struct move *m, int axis, double move_time + , const struct shaper_pulses *sp + , const struct smoother *sm , double *pos_integral, double *pa_velocity_integral) { *pos_integral = *pa_velocity_integral = 0.; @@ -132,7 +106,7 @@ shaper_pa_range_integrate(struct move *m, int axis, double move_time for (i = 0; i < num_pulses; ++i) { double t = sp->pulses[i].t, a = sp->pulses[i].a; double p_pos_int, p_pa_vel_int; - pa_range_integrate(m, axis, move_time + t, hst, + pa_range_integrate(m, axis, move_time + t, sm, &p_pos_int, &p_pa_vel_int); *pos_integral += a * p_pos_int; *pa_velocity_integral += a * p_pa_vel_int; @@ -157,9 +131,10 @@ typedef double (*pressure_advance_func)( struct extruder_stepper { struct stepper_kinematics sk; struct shaper_pulses sp[3]; + struct smoother sm[3]; struct pressure_advance_params pa_params; pressure_advance_func pa_func; - double time_offset, half_smooth_time, inv_half_smooth_time2; + double time_offset; }; double __visible @@ -207,37 +182,34 @@ extruder_calc_position(struct stepper_kinematics *sk, struct move *m move_time -= m->move_t; m = list_next_entry(m, node); } - double hst = es->half_smooth_time; int i; struct coord e_pos, pa_vel; double move_dist = move_get_distance(m, move_time); for (i = 0; i < 3; ++i) { int axis = 'x' + i; - struct shaper_pulses* sp = &es->sp[i]; + const struct shaper_pulses* sp = &es->sp[i]; + const struct smoother* sm = &es->sm[i]; int num_pulses = sp->num_pulses; - if (!hst) { + if (!sm->hst) { e_pos.axis[i] = num_pulses ? shaper_calc_position(m, axis, move_time, sp) : m->start_pos.axis[i] + m->axes_r.axis[i] * move_dist; pa_vel.axis[i] = 0.; } else { if (num_pulses) { - shaper_pa_range_integrate(m, axis, move_time, hst, sp, + shaper_pa_range_integrate(m, axis, move_time, sp, sm, &e_pos.axis[i], &pa_vel.axis[i]); } else { - pa_range_integrate(m, axis, move_time, hst, + pa_range_integrate(m, axis, move_time, sm, &e_pos.axis[i], &pa_vel.axis[i]); } - e_pos.axis[i] *= es->inv_half_smooth_time2; - pa_vel.axis[i] *= es->inv_half_smooth_time2; } } double position = e_pos.x + e_pos.y + e_pos.z; - if (!hst) - return position; double pa_velocity = pa_vel.x + pa_vel.y + pa_vel.z; - if (pa_velocity < 0.) pa_velocity = 0.; - return es->pa_func(position, pa_velocity, &es->pa_params); + if (pa_velocity > 0.) + return es->pa_func(position, pa_velocity, &es->pa_params); + return position; } static void @@ -245,18 +217,18 @@ extruder_note_generation_time(struct extruder_stepper *es) { double pre_active = 0., post_active = 0.; int i; - for (i = 0; i < 2; ++i) { + for (i = 0; i < 3; ++i) { struct shaper_pulses* sp = &es->sp[i]; - if (!es->sp[i].num_pulses) continue; - pre_active = sp->pulses[sp->num_pulses-1].t > pre_active - ? sp->pulses[sp->num_pulses-1].t : pre_active; - post_active = -sp->pulses[0].t > post_active - ? -sp->pulses[0].t : post_active; + const struct smoother* sm = &es->sm[i]; + double pre_active_axis = sm->hst + sm->t_offs + es->time_offset + + (sp->num_pulses ? sp->pulses[sp->num_pulses-1].t : 0.); + double post_active_axis = sm->hst - sm->t_offs - es->time_offset + + (sp->num_pulses ? -sp->pulses[0].t : 0.); + if (pre_active_axis > pre_active) + pre_active = pre_active_axis; + if (post_active_axis > post_active) + post_active = post_active_axis; } - pre_active += es->half_smooth_time + es->time_offset; - if (pre_active < 0.) pre_active = 0.; - post_active += es->half_smooth_time - es->time_offset; - if (post_active < 0.) post_active = 0.; es->sk.gen_steps_pre_active = pre_active; es->sk.gen_steps_post_active = post_active; } @@ -264,17 +236,12 @@ extruder_note_generation_time(struct extruder_stepper *es) void __visible extruder_set_pressure_advance(struct stepper_kinematics *sk , int n_params, double params[] - , double smooth_time, double time_offset) + , double time_offset) { struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); - double hst = smooth_time * .5; - es->half_smooth_time = hst; es->time_offset = time_offset; - extruder_note_generation_time(es); - if (! hst) - return; - es->inv_half_smooth_time2 = 1. / (hst * hst); memset(&es->pa_params, 0, sizeof(es->pa_params)); + extruder_note_generation_time(es); if (n_params < 0 || n_params > ARRAY_SIZE(es->pa_params.params)) return; memcpy(&es->pa_params, params, n_params * sizeof(params[0])); @@ -302,6 +269,19 @@ extruder_set_shaper_params(struct stepper_kinematics *sk, char axis return status; } +int __visible +extruder_set_smoothing_params(struct stepper_kinematics *sk, char axis + , int n, double a[], double t_sm) +{ + if (axis != 'x' && axis != 'y' && axis != 'z') + return -1; + struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); + struct smoother *sm = &es->sm[axis-'x']; + int status = init_smoother(n, a, t_sm, sm); + extruder_note_generation_time(es); + return status; +} + double __visible extruder_get_step_gen_window(struct stepper_kinematics *sk) { diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c index 6a2f9ec71..6b9f53e7b 100644 --- a/klippy/chelper/kin_shaper.c +++ b/klippy/chelper/kin_shaper.c @@ -10,6 +10,7 @@ #include // malloc #include // memset #include "compiler.h" // __visible +#include "integrate.h" // integrate_weighted #include "itersolve.h" // struct stepper_kinematics #include "kin_shaper.h" // struct shaper_pulses #include "trapq.h" // struct move @@ -60,7 +61,7 @@ init_shaper(int n, double a[], double t[], struct shaper_pulses *sp) ****************************************************************/ static inline double -get_axis_position(struct move *m, int axis, double move_time) +get_axis_position(const struct move *m, int axis, double move_time) { double axis_r = m->axes_r.axis[axis - 'x']; double start_pos = m->start_pos.axis[axis - 'x']; @@ -69,7 +70,7 @@ get_axis_position(struct move *m, int axis, double move_time) } static inline double -get_axis_position_across_moves(struct move *m, int axis, double time) +get_axis_position_across_moves(const struct move *m, int axis, double time) { while (likely(time < 0.)) { m = list_prev_entry(m, node); @@ -84,8 +85,8 @@ get_axis_position_across_moves(struct move *m, int axis, double time) // Calculate the position from the convolution of the shaper with input signal inline double -shaper_calc_position(struct move *m, int axis, double move_time - , struct shaper_pulses *sp) +shaper_calc_position(const struct move *m, int axis, double move_time + , const struct shaper_pulses *sp) { double res = 0.; int num_pulses = sp->num_pulses, i; @@ -96,6 +97,68 @@ shaper_calc_position(struct move *m, int axis, double move_time return res; } +/**************************************************************** + * Generic position calculation via smoother integration + ****************************************************************/ + +// Calculate the definitive integral on a part of a move +static double +move_integrate(const struct move *m, int axis, double start, double end + , double t0, const struct smoother *sm) +{ + if (start < 0.) + start = 0.; + if (end > m->move_t) + end = m->move_t; + double axis_r = m->axes_r.axis[axis - 'x']; + double start_pos = m->start_pos.axis[axis - 'x']; + double res = integrate_weighted(sm, start_pos, + axis_r * m->start_v, axis_r * m->half_accel, + start, end, t0); + return res; +} + +// Calculate the definitive integral over a range of moves +static double +range_integrate(const struct move *m, int axis, double move_time + , const struct smoother *sm) +{ + move_time += sm->t_offs; + while (unlikely(move_time < 0.)) { + m = list_prev_entry(m, node); + move_time += m->move_t; + } + while (unlikely(move_time > m->move_t)) { + move_time -= m->move_t; + m = list_next_entry(m, node); + } + // Calculate integral for the current move + double start = move_time - sm->hst, end = move_time + sm->hst; + double res = move_integrate(m, axis, start, end, /*t0=*/move_time, sm); + // Integrate over previous moves + const struct move *prev = m; + while (unlikely(start < 0.)) { + prev = list_prev_entry(prev, node); + start += prev->move_t; + res += move_integrate(prev, axis, start, prev->move_t, + /*t0=*/start + sm->hst, sm); + } + // Integrate over future moves + while (unlikely(end > m->move_t)) { + end -= m->move_t; + m = list_next_entry(m, node); + res += move_integrate(m, axis, 0., end, /*t0=*/end - sm->hst, sm); + } + return res; +} + +// Calculate average position using the specified smoother +static inline double +smoother_calc_position(const struct move *m, int axis, double move_time + , const struct smoother *sm) +{ + return range_integrate(m, axis, move_time, sm); +} /**************************************************************** * Kinematics-related shaper code @@ -107,7 +170,8 @@ struct input_shaper { struct stepper_kinematics sk; struct stepper_kinematics *orig_sk; struct move m; - struct shaper_pulses sx, sy; + struct shaper_pulses sp_x, sp_y; + struct smoother sm_x, sm_y; }; // Optimized calc_position when only x axis is needed @@ -116,9 +180,11 @@ shaper_x_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) { struct input_shaper *is = container_of(sk, struct input_shaper, sk); - if (!is->sx.num_pulses) + if (!is->sp_x.num_pulses && !is->sm_x.hst) return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); - is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx); + is->m.start_pos.x = is->sp_x.num_pulses + ? shaper_calc_position(m, 'x', move_time, &is->sp_x) + : smoother_calc_position(m, 'x', move_time, &is->sm_x); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -128,9 +194,11 @@ shaper_y_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) { struct input_shaper *is = container_of(sk, struct input_shaper, sk); - if (!is->sy.num_pulses) + if (!is->sp_y.num_pulses && !is->sm_y.hst) return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); - is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy); + is->m.start_pos.y = is->sp_y.num_pulses + ? shaper_calc_position(m, 'y', move_time, &is->sp_y) + : smoother_calc_position(m, 'y', move_time, &is->sm_y); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -140,13 +208,18 @@ shaper_xy_calc_position(struct stepper_kinematics *sk, struct move *m , double move_time) { struct input_shaper *is = container_of(sk, struct input_shaper, sk); - if (!is->sx.num_pulses && !is->sy.num_pulses) + if (!is->sp_x.num_pulses && !is->sp_y.num_pulses + && !is->sm_x.hst && !is->sm_y.hst) return is->orig_sk->calc_position_cb(is->orig_sk, m, move_time); is->m.start_pos = move_get_coord(m, move_time); - if (is->sx.num_pulses) - is->m.start_pos.x = shaper_calc_position(m, 'x', move_time, &is->sx); - if (is->sy.num_pulses) - is->m.start_pos.y = shaper_calc_position(m, 'y', move_time, &is->sy); + if (is->sp_x.num_pulses || is->sm_x.hst) + is->m.start_pos.x = is->sp_x.num_pulses + ? shaper_calc_position(m, 'x', move_time, &is->sp_x) + : smoother_calc_position(m, 'x', move_time, &is->sm_x); + if (is->sp_y.num_pulses || is->sm_y.hst) + is->m.start_pos.y = is->sp_y.num_pulses + ? shaper_calc_position(m, 'y', move_time, &is->sp_y) + : smoother_calc_position(m, 'y', move_time, &is->sm_y); return is->orig_sk->calc_position_cb(is->orig_sk, &is->m, DUMMY_T); } @@ -175,15 +248,25 @@ static void shaper_note_generation_time(struct input_shaper *is) { double pre_active = 0., post_active = 0.; - if ((is->sk.active_flags & AF_X) && is->sx.num_pulses) { - pre_active = is->sx.pulses[is->sx.num_pulses-1].t; - post_active = -is->sx.pulses[0].t; + if ((is->sk.active_flags & AF_X) && is->sp_x.num_pulses) { + pre_active = is->sp_x.pulses[is->sp_x.num_pulses-1].t; + post_active = -is->sp_x.pulses[0].t; + } else if ((is->sk.active_flags & AF_X) && is->sm_x.hst) { + pre_active = is->sm_x.hst + is->sm_x.t_offs; + if (pre_active < 0.) pre_active = 0.; + post_active = is->sm_x.hst - is->sm_x.t_offs; + if (post_active < 0.) post_active = 0.; } - if ((is->sk.active_flags & AF_Y) && is->sy.num_pulses) { - pre_active = is->sy.pulses[is->sy.num_pulses-1].t > pre_active - ? is->sy.pulses[is->sy.num_pulses-1].t : pre_active; - post_active = -is->sy.pulses[0].t > post_active - ? -is->sy.pulses[0].t : post_active; + if ((is->sk.active_flags & AF_Y) && is->sp_y.num_pulses) { + pre_active = is->sp_y.pulses[is->sp_y.num_pulses-1].t > pre_active + ? is->sp_y.pulses[is->sp_y.num_pulses-1].t : pre_active; + post_active = -is->sp_y.pulses[0].t > post_active + ? -is->sp_y.pulses[0].t : post_active; + } else if ((is->sk.active_flags & AF_Y) && is->sm_y.hst) { + pre_active = is->sm_y.hst + is->sm_y.t_offs > pre_active + ? is->sm_y.hst + is->sm_y.t_offs : pre_active; + post_active = is->sm_y.hst - is->sm_y.t_offs > post_active + ? is->sm_y.hst - is->sm_y.t_offs : post_active; } is->sk.gen_steps_pre_active = pre_active; is->sk.gen_steps_post_active = post_active; @@ -196,11 +279,31 @@ input_shaper_set_shaper_params(struct stepper_kinematics *sk, char axis if (axis != 'x' && axis != 'y') return -1; struct input_shaper *is = container_of(sk, struct input_shaper, sk); - struct shaper_pulses *sp = axis == 'x' ? &is->sx : &is->sy; + struct shaper_pulses *sp = axis == 'x' ? &is->sp_x : &is->sp_y; + struct smoother *sm = axis == 'x' ? &is->sm_x : &is->sm_y; int status = 0; // Ignore input shaper update if the axis is not active if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y)) { status = init_shaper(n, a, t, sp); + memset(sm, 0, sizeof(*sm)); + shaper_note_generation_time(is); + } + return status; +} + +int __visible +input_shaper_set_smoother_params(struct stepper_kinematics *sk, char axis + , int n, double a[], double t_sm) +{ + if (axis != 'x' && axis != 'y') + return -1; + struct input_shaper *is = container_of(sk, struct input_shaper, sk); + struct shaper_pulses *sp = axis == 'x' ? &is->sp_x : &is->sp_y; + struct smoother *sm = axis == 'x' ? &is->sm_x : &is->sm_y; + int status = 0; + if (is->orig_sk->active_flags & (axis == 'x' ? AF_X : AF_Y)) { + status = init_smoother(n, a, t_sm, sm); + sp->num_pulses = 0; shaper_note_generation_time(is); } return status; diff --git a/klippy/chelper/kin_shaper.h b/klippy/chelper/kin_shaper.h index 16012c4a4..041d40b17 100644 --- a/klippy/chelper/kin_shaper.h +++ b/klippy/chelper/kin_shaper.h @@ -11,7 +11,7 @@ struct shaper_pulses { struct move; int init_shaper(int n, double a[], double t[], struct shaper_pulses *sp); -double shaper_calc_position(struct move *m, int axis, double move_time - , struct shaper_pulses *sp); +double shaper_calc_position(const struct move *m, int axis, double move_time + , const struct shaper_pulses *sp); #endif // kin_shaper.h diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index 9b1b501b4..4d7bab405 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -22,14 +22,14 @@ move_alloc(void) // Return the distance moved given a time in a move inline double -move_get_distance(struct move *m, double move_time) +move_get_distance(const struct move *m, double move_time) { return (m->start_v + m->half_accel * move_time) * move_time; } // Return the XYZ coordinates given a time in a move inline struct coord -move_get_coord(struct move *m, double move_time) +move_get_coord(const struct move *m, double move_time) { double move_dist = move_get_distance(m, move_time); return (struct coord) { diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index bd8f4e8c2..f00e52a81 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -32,8 +32,8 @@ struct pull_move { }; struct move *move_alloc(void); -double move_get_distance(struct move *m, double move_time); -struct coord move_get_coord(struct move *m, double move_time); +double move_get_distance(const struct move *m, double move_time); +struct coord move_get_coord(const struct move *m, double move_time); struct trapq *trapq_alloc(void); void trapq_free(struct trapq *tq); void trapq_check_sentinels(struct trapq *tq); diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index a4362c988..7be340cef 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -140,6 +140,10 @@ def get_type(self): return self.params.get_type() def get_axis(self): return self.params.get_axis() + def is_smoothing(self): + return False + def is_enabled(self): + return self.n > 0 def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.A, self.T = self.params.get_shaper() @@ -156,6 +160,10 @@ def update_stepper_kinematics(self, sk): def update_extruder_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() + # Make sure to disable any active input smoothing + coeffs, smooth_time = [], 0. + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, len(coeffs), coeffs, smooth_time) == 0 success = ffi_lib.extruder_set_shaper_params( sk, axis, self.n, self.A, self.T) == 0 if not success: @@ -183,10 +191,109 @@ def report(self, gcmd): for (key, value) in self.params.get_status().items()]) gcmd.respond_info(info) +class CustomInputSmootherParams: + SHAPER_TYPE = 'smoother' + def __init__(self, axis, config): + self.axis = axis + self.smooth_time, self.coeffs = 0., [1.] + if config is not None: + self.smooth_time = config.getfloat('smooth_time_' + axis, + self.smooth_time, minval=0.) + self.coeffs = list(reversed(config.getfloatlist('coeffs_' + axis, + self.coeffs))) + def get_type(self): + return self.SHAPER_TYPE + def get_axis(self): + return self.axis + def update(self, shaper_type, gcmd): + if shaper_type != self.SHAPER_TYPE: + raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) + axis = self.axis.upper() + self.smooth_time = gcmd.get_float('SMOOTH_TIME_' + axis, + self.smooth_time) + coeffs_str = gcmd.get('COEFFS_' + axis, None) + if coeffs_str is not None: + try: + coeffs = parse_float_list(coeffs_str) + coeffs.reverse() + except: + raise gcmd.error("Invalid format for COEFFS parameter") + self.coeffs = coeffs + def get_smoother(self): + return len(self.coeffs), self.coeffs, self.smooth_time + def get_status(self): + return collections.OrderedDict([ + ('shaper_type', self.SHAPER_TYPE), + ('shaper_coeffs', ','.join(['%.9e' % (a,) + for a in reversed(self.coeffs)])), + ('shaper_smooth_time', self.smooth_time)]) + +class AxisInputSmoother: + def __init__(self, params): + self.params = params + self.n, self.coeffs, self.smooth_time = params.get_smoother() + self.saved_smooth_time = 0. + def get_name(self): + return 'smoother_' + self.axis + def get_type(self): + return self.params.get_type() + def get_axis(self): + return self.params.get_axis() + def is_smoothing(self): + return True + def is_enabled(self): + return self.smooth_time > 0. + def update(self, shaper_type, gcmd): + self.params.update(shaper_type, gcmd) + self.n, self.coeffs, self.smooth_time = self.params.get_smoother() + def update_stepper_kinematics(self, sk): + ffi_main, ffi_lib = chelper.get_ffi() + axis = self.get_axis().encode() + success = ffi_lib.input_shaper_set_smoother_params( + sk, axis, self.n, self.coeffs, self.smooth_time) == 0 + if not success: + self.disable_shaping() + ffi_lib.input_shaper_set_smoother_params( + sk, axis, self.n, self.coeffs, self.smooth_time) + return success + def update_extruder_kinematics(self, sk): + ffi_main, ffi_lib = chelper.get_ffi() + axis = self.get_axis().encode() + # Make sure to disable any active input shaping + A, T = shaper_defs.get_none_shaper() + ffi_lib.extruder_set_shaper_params(sk, axis, len(A), A, T) + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, self.n, self.coeffs, self.smooth_time) == 0 + if not success: + self.disable_shaping() + ffi_lib.extruder_set_smoothing_params( + sk, axis, self.n, self.coeffs, self.smooth_time) + return success + def disable_shaping(self): + was_enabled = False + if self.smooth_time: + self.saved_smooth_time = self.smooth_time + was_enabled = True + self.smooth_time = 0. + return was_enabled + def enable_shaping(self): + if not self.saved_smooth_time: + # Input smoother was not disabled + return False + self.smooth_time = self.saved_smooth_time + self.saved_smooth_time = 0. + return True + def report(self, gcmd): + info = ' '.join(["%s_%s:%s" % (key, self.get_axis(), value) + for (key, value) in self.params.get_status().items()]) + gcmd.respond_info(info) + class ShaperFactory: def __init__(self): pass def _create_shaper(self, axis, type_name, config=None): + if type_name == CustomInputSmootherParams.SHAPER_TYPE: + return AxisInputSmoother(CustomInputSmootherParams(axis, config)) if type_name == CustomInputShaperParams.SHAPER_TYPE: return AxisInputShaper(CustomInputShaperParams(axis, config)) if type_name in TypedInputShaperParams.shapers: diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 2e4b306bb..52de6beca 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -6,6 +6,41 @@ import math, logging import stepper, chelper +class ExtruderSmoother: + def __init__(self, config, pa_model): + self.smooth_time = config.getfloat( + 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) + # A 4-th order smoothing function that goes to 0 together with + # its derivative at the ends of the smoothing interval + self.a = [15./8., 0., -15., 0., 30.] + self.pa_model = pa_model + self.axes = ['x', 'y', 'z'] + def update(self, gcmd): + self.smooth_time = gcmd.get_float('SMOOTH_TIME', self.smooth_time) + def update_pa_model(self, pa_model): + self.pa_model = pa_model + def enable_axis(self, axis): + if axis not in self.axes: + self.axes.append(axis) + def disable_axis(self, axis): + if axis in self.axes: + self.axes.remove(axis) + def update_extruder_kinematics(self, extruder_sk): + ffi_main, ffi_lib = chelper.get_ffi() + n = len(self.a) + success = True + smooth_time = self.smooth_time if self.pa_model.enabled() else 0. + for axis in self.axes: + if not ffi_lib.extruder_set_smoothing_params( + extruder_sk, axis.encode(), n, self.a, + smooth_time) == 0: + success = False + return success + def get_status(self, eventtime): + return {'smooth_time': self.smooth_time} + def get_msg(self): + return "pressure_advance_smooth_time: %.6f" % (self.smooth_time,) + class PALinearModel: name = 'linear' def __init__(self, config=None): @@ -103,8 +138,7 @@ def __init__(self, config): self.pa_model = config.getchoice('pressure_advance_model', self.pa_models, PALinearModel.name)(config) - self.pressure_advance_smooth_time = config.getfloat( - 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) + self.smoother = ExtruderSmoother(config, self.pa_model) self.pressure_advance_time_offset = config.getfloat( 'pressure_advance_time_offset', 0.0, minval=-0.2, maxval=0.2) # Setup stepper @@ -168,24 +202,25 @@ def __init__(self, config): ) def _handle_connect(self): - self.toolhead = self.printer.lookup_object('toolhead') - self.toolhead.register_step_generator(self.stepper.generate_steps) + toolhead = self.printer.lookup_object('toolhead') + toolhead.register_step_generator(self.stepper.generate_steps) self._update_pressure_advance(self.pa_model, - self.pressure_advance_smooth_time, self.pressure_advance_time_offset) + self.smoother.update_extruder_kinematics(self.sk_extruder) def get_status(self, eventtime): sts = {'pressure_advance_model': self.pa_model.name, - 'smooth_time': self.pressure_advance_smooth_time, 'time_offset': self.pressure_advance_time_offset, 'motion_queue': self.motion_queue} sts.update(self.pa_model.get_status(eventtime)) + sts.update(self.smoother.get_status(eventtime)) return sts def find_past_position(self, print_time): mcu_pos = self.stepper.get_past_mcu_position(print_time) return self.stepper.mcu_to_commanded_position(mcu_pos) def sync_to_extruder(self, extruder_name): - self.toolhead.flush_step_generation() + toolhead = self.printer.lookup_object('toolhead') + toolhead.flush_step_generation() if not extruder_name: if self.extruder is not None: self.extruder.unlink_extruder_stepper(self) @@ -199,8 +234,9 @@ def sync_to_extruder(self, extruder_name): extruder.link_extruder_stepper(self) self.motion_queue = extruder_name self.extruder = extruder - def _update_pressure_advance(self, pa_model, smooth_time, time_offset): - self.toolhead.flush_step_generation() + def _update_pressure_advance(self, pa_model, time_offset): + toolhead = self.printer.lookup_object('toolhead') + toolhead.flush_step_generation() ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) if self.pa_model.name != pa_model.name: @@ -209,24 +245,32 @@ def _update_pressure_advance(self, pa_model, smooth_time, time_offset): pa_func) pa_params = pa_model.get_pa_params() ffi_lib.extruder_set_pressure_advance( - self.sk_extruder, len(pa_params), pa_params, - smooth_time, time_offset) + self.sk_extruder, len(pa_params), pa_params, time_offset) + self.smoother.update_pa_model(pa_model) + self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) if old_delay != new_delay: - self.toolhead.note_step_generation_scan_time(new_delay, old_delay) + toolhead.note_step_generation_scan_time(new_delay, old_delay) self.pa_model = pa_model - self.pressure_advance_smooth_time = smooth_time self.pressure_advance_time_offset = time_offset def update_input_shaping(self, shapers): ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) failed_shapers = [] - for shaper in self.shapers: + for shaper in shapers: if not shaper.update_extruder_kinematics(self.sk_extruder): failed_shapers.append(shaper) + # Pressure advance requires extruder smoothing, make sure that + # some smoothing is enabled + if shaper.is_smoothing() and shaper.is_enabled(): + self.smoother.disable_axis(shaper.get_axis()) + elif not shaper.is_smoothing() or not shaper.is_enabled(): + self.smoother.enable_axis(shaper.get_axis()) + self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) + toolhead = self.printer.lookup_object('toolhead') if old_delay != new_delay: - self.toolhead.note_step_generation_scan_time(new_delay, old_delay) + toolhead.note_step_generation_scan_time(new_delay, old_delay) return failed_shapers cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" @@ -248,18 +292,16 @@ def cmd_SET_PRESSURE_ADVANCE(self, gcmd): if pa_model_name != self.pa_model.name: pa_model = self.pa_models[pa_model_name]() pa_model.update(gcmd) - smooth_time = gcmd.get_float('SMOOTH_TIME', - self.pressure_advance_smooth_time, - minval=0., maxval=.200) + self.smoother.update(gcmd) time_offset = gcmd.get_float('TIME_OFFSET', self.pressure_advance_time_offset, minval=-0.2, maxval=0.2) - self._update_pressure_advance(pa_model, smooth_time, time_offset) + self._update_pressure_advance(pa_model, time_offset) msg = ("pressure_advance_model: %s\n" % (pa_model.name,) + - pa_model.get_msg() + - "\npressure_advance_smooth_time: %.6f" + pa_model.get_msg() + "\n" + + self.smoother.get_msg() + "\npressure_advance_time_offset: %.6f" - % (smooth_time, time_offset)) + % (time_offset,)) self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg)) gcmd.respond_info(msg, log=False) From 1ef3a53f5bb69526325df74ed401a9c15610d0ff Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Thu, 27 Apr 2023 00:59:38 +0200 Subject: [PATCH 20/35] input_shaper: Added some predefined input smoothers Signed-off-by: Dmitry Butyugin --- klippy/extras/input_shaper.py | 40 ++++++++++++- klippy/extras/shaper_defs.py | 107 ++++++++++++++++++++++++++++++++-- 2 files changed, 142 insertions(+), 5 deletions(-) diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 7be340cef..7fcb948a7 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -191,11 +191,46 @@ def report(self, gcmd): for (key, value) in self.params.get_status().items()]) gcmd.respond_info(info) +class TypedInputSmootherParams: + smoothers = {s.name : s.init_func for s in shaper_defs.INPUT_SMOOTHERS} + def __init__(self, axis, smoother_type, config): + self.axis = axis + self.smoother_type = smoother_type + self.smoother_freq = 0. + if config is not None: + if smoother_type not in self.smoothers: + raise config.error( + 'Unsupported shaper type: %s' % (smoother_type,)) + self.smoother_freq = config.getfloat('smoother_freq_' + axis, + self.smoother_freq, minval=0.) + def get_type(self): + return self.smoother_type + def get_axis(self): + return self.axis + def update(self, smoother_type, gcmd): + if smoother_type not in self.smoothers: + raise gcmd.error('Unsupported shaper type: %s' % (smoother_type,)) + axis = self.axis.upper() + self.smoother_freq = gcmd.get_float('SMOOTHER_FREQ_' + axis, + self.smoother_freq, minval=0.) + self.smoother_type = smoother_type + def get_smoother(self): + if not self.smoother_freq: + C, tsm = shaper_defs.get_none_smoother() + else: + C, tsm = self.smoothers[self.smoother_type](self.smoother_freq, + normalize_coeffs=False) + return len(C), C, tsm + def get_status(self): + return collections.OrderedDict([ + ('shaper_type', self.smoother_type), + ('smoother_freq', '%.3f' % (self.smoother_freq,))]) + class CustomInputSmootherParams: SHAPER_TYPE = 'smoother' def __init__(self, axis, config): self.axis = axis - self.smooth_time, self.coeffs = 0., [1.] + self.coeffs, self.smooth_time = shaper_defs.get_none_smoother() if config is not None: self.smooth_time = config.getfloat('smooth_time_' + axis, self.smooth_time, minval=0.) @@ -299,6 +334,9 @@ def _create_shaper(self, axis, type_name, config=None): if type_name in TypedInputShaperParams.shapers: return AxisInputShaper( TypedInputShaperParams(axis, type_name, config)) + if type_name in TypedInputSmootherParams.smoothers: + return AxisInputSmoother( + TypedInputSmootherParams(axis, type_name, config)) return None def create_shaper(self, axis, config): shaper_type = config.get('shaper_type', 'mzv') diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index 2deab7645..fe172de79 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -1,6 +1,6 @@ # Definitions of the supported input shapers # -# Copyright (C) 2020-2021 Dmitry Butyugin +# Copyright (C) 2020-2023 Dmitry Butyugin # # This file may be distributed under the terms of the GNU GPLv3 license. import collections, math @@ -9,9 +9,9 @@ DEFAULT_DAMPING_RATIO = 0.1 InputShaperCfg = collections.namedtuple( - "InputShaperCfg", ("name", "init_func", "min_freq") -) - + 'InputShaperCfg', ('name', 'init_func', 'min_freq')) +InputSmootherCfg = collections.namedtuple( + 'InputSmootherCfg', ('name', 'init_func', 'min_freq')) def get_none_shaper(): return ([], []) @@ -101,6 +101,95 @@ def get_3hump_ei_shaper(shaper_freq, damping_ratio): T = [0.0, 0.5 * t_d, t_d, 1.5 * t_d, 2.0 * t_d] return (A, T) +def init_smoother(coeffs, smooth_time, normalize_coeffs): + if not normalize_coeffs: + return (list(reversed(coeffs)), smooth_time) + inv_t_sm = inv_t_sm_n = 1. / smooth_time + n = len(coeffs) + c = [0] * n + for i in range(n-1,-1,-1): + c[n-i-1] = coeffs[i] * inv_t_sm_n + inv_t_sm_n *= inv_t_sm + return (c, smooth_time) + +def get_none_smoother(): + return ([1.], 0.) + +# Input smoother is a polynomial function w(t) which is convolved with the input +# trajectory x(t) as x_sm(T) = integral(x(t) * w(T-t) * dt, t=[T-T_sm...T+T_sm]) +# to obtain a modified trajectory which has low residual vibrations in a certain +# frequency range. Similar to discrete input shapers, the amplitude of residual +# vibrations of a step input signal can be calculated as +# *) V_c = integral(exp(-zeta * omega * t) * cos(omega_d * t) * +# * w(T_sm/2 - t) * dt, t=[0...T_sm]) +# *) V_s = integral(exp(-zeta * omega * t) * sin(omega_d * t) * +# * w(T_sm/2 - t) * dt, t=[0...T_sm]) +# *) V = sqrt(V_c^2 + V_s^) +# +# The input smoothers defined below were calculated and optimized in the Maxima +# algebra system by calculating V(w, T_sm, omega, zeta) in a closed form for +# several (depending on smoother) omega_i, zeta_i pairs and optimizing T_sm, +# omega_i, and zeta_i in order to obtain a polynomial w(t) with a minimal T_sm +# such that w(t) >= 0 and V(omega) <= 0.05 in a certain frequency range. +# +# Additional constraints that were enforced for w(t) during optimization: +# *) integral(w(t) * dt, t=[-T_sm/2...T_sm/2]) = 1 +# *) w(-T_sm/2) = w(T_sm/2) = 0 +# The first condition ensures unity norm of w(t) and the second one makes the +# input smoothers usable as extruder smoothers (for the ease of synchronization +# of the extruder with the toolhead motion). + +def get_zv_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-118.4265334338076,5.861885495127615,29.52796003014231, + -1.465471373781904,0.01966833207740377] + return init_smoother(coeffs, 0.8025 / shaper_freq, normalize_coeffs) + +def get_mzv20_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-1928.625991372561,174.47238885576,680.8819871456237, + -54.08725118644957,-55.28572183696386,2.617288493127363, + 1.401087377835678] + return init_smoother(coeffs, 0.85175 / shaper_freq, normalize_coeffs) + +def get_mzv16_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-1938.774071514084,157.7065914461193,711.7324202087348, + -48.34288782208795,-63.72538103023892,2.22905999013954, + 1.741413861921329] + return init_smoother(coeffs, 0.9665 / shaper_freq, normalize_coeffs) + +def get_2hump_ei_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-22525.88434486782,2524.826047114184,10554.22832043971, + -1051.778387878068,-1475.914693073253,121.2177946817349, + 57.95603221424528,-4.018706414213658,0.8375784787864095] + return init_smoother(coeffs, 1.14875 / shaper_freq, normalize_coeffs) + +def get_si_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-121313.2032178889,26921.56924860698,63029.77630233325, + -13293.49355097946,-10648.58182242004,2050.103214332665, + 666.126576474768,-102.9883580154977,-17.0691108398123, + 0.1640954647660932,1.278318992073449] + return init_smoother(coeffs, 1.2 / shaper_freq, normalize_coeffs) + +def get_nei_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [4883445.221306241,-720494.0751766556,-3148819.580240736, + 424777.3712490448,721441.1426576816,-85447.8333563426, + -70054.38903665643,6764.372246231036,2669.737943114452, + -180.6374244922258,-30.4660136303673,1.82937577459115] + return init_smoother(coeffs, 1.42 / shaper_freq, normalize_coeffs) + +def get_nsi_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [1.576037300103821e7,-3450496.635175148,-1.138914512941258e7, + 2263562.40616947,3103579.174607452,-537674.9863778015, + -397766.323762382,56331.41234254206,24270.29707072854, + -2532.279663870225,-598.7901101230622,36.92208279328751, + -0.9886556775166597,1.047798776818597] + return init_smoother(coeffs, 1.58 / shaper_freq, normalize_coeffs) # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ @@ -111,3 +200,13 @@ def get_3hump_ei_shaper(shaper_freq, damping_ratio): InputShaperCfg("2hump_ei", get_2hump_ei_shaper, min_freq=39.0), InputShaperCfg("3hump_ei", get_3hump_ei_shaper, min_freq=48.0), ] + +INPUT_SMOOTHERS = [ + InputSmootherCfg('smooth_zv', get_zv_smoother, min_freq=21.), + InputSmootherCfg('smooth_mzv20', get_mzv20_smoother, min_freq=23.), + InputSmootherCfg('smooth_mzv16', get_mzv16_smoother, min_freq=23.), + InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=23.), + InputSmootherCfg('smooth_si', get_si_smoother, min_freq=23.), + InputSmootherCfg('smooth_nei', get_nei_smoother, min_freq=23.), + InputSmootherCfg('smooth_nsi', get_nsi_smoother, min_freq=23.), +] From 84e12286d54924c547dd9d796217504b2d69fd46 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sat, 6 May 2023 23:48:25 +0200 Subject: [PATCH 21/35] integrate: Slightly more optimized versions of smoother integration Also added a version for symmetric smoother integration (for extruder) Signed-off-by: Dmitry Butyugin --- klippy/chelper/integrate.c | 121 ++++++++++++++++++++++++---------- klippy/chelper/integrate.h | 13 ++-- klippy/chelper/kin_extruder.c | 32 ++++----- klippy/chelper/kin_shaper.c | 28 ++------ 4 files changed, 117 insertions(+), 77 deletions(-) diff --git a/klippy/chelper/integrate.c b/klippy/chelper/integrate.c index 6f5845337..91eec75db 100644 --- a/klippy/chelper/integrate.c +++ b/klippy/chelper/integrate.c @@ -5,8 +5,9 @@ // // This file may be distributed under the terms of the GNU GPLv3 license. -#include "compiler.h" +#include "compiler.h" // unlikely #include "integrate.h" +#include "trapq.h" // struct move #include @@ -19,53 +20,93 @@ static double coeffs[] = { 1./11., 1./12., 1./13., 1./14., 1./15., 1./16., 1./17., 1./18., 1./19., }; -inline static double -integrate_nth_order(int n, const double c[], double start, double end - , double a0, double a1, double a2) +inline static void +integrate_smoother(const struct smoother* sm, double start, double end + , double* s0, double* s1, double* s2) { + int n = sm->n; double u = start + end; - double v = u * .5; double end_n = end; // First 2 iterations with i == 0 and 1 - double sum0 = c[0] + c[1] * v, sum1 = c[0] * v, sum2 = 0.; + double sum0 = sm->c0[0] + sm->c0[1] * u, sum1 = sm->c1[0] * u, sum2 = 0.; int i; for (i = 2; i < n; ++i) { end_n *= end; u = u * start + end_n; - v = u * coeffs[i]; - sum2 += c[i-2] * v; - sum1 += c[i-1] * v; - sum0 += c[i] * v; + sum2 += sm->c2[i-2] * u; + sum1 += sm->c1[i-1] * u; + sum0 += sm->c0[i] * u; } // Remainging 2 iterations with i == n and n+1 end_n *= end; u = u * start + end_n; - v = u * coeffs[n]; - sum2 += c[n-2] * v; - sum1 += c[n-1] * v; + sum2 += sm->c2[n-2] * u; + sum1 += sm->c1[n-1] * u; end_n *= end; u = u * start + end_n; - v = u * coeffs[n+1]; - sum2 += c[n-1] * v; + sum2 += sm->c2[n-1] * u; - double delta_t = end - start; - double avg_val = a0 * sum0 + a1 * sum1 + a2 * sum2; - return avg_val * delta_t; + *s0 = sum0; + *s1 = sum1; + *s2 = sum2; +} + +inline static void +integrate_smoother_symm(const struct smoother* sm, double start, double end + , double* s0, double* s1, double* s2) +{ + int n = sm->n; + double u = start + end; + double end_n = end; + // First 2 iterations with i == 0 and 1 + double sum0 = sm->c0[0], sum1 = sm->c1[0] * u, sum2 = 0.; + int i; + for (i = 2; i < n; i += 2) { + end_n *= end; + u = u * start + end_n; + sum2 += sm->c2[i-2] * u; + sum0 += sm->c0[i] * u; + end_n *= end; + u = u * start + end_n; + sum1 += sm->c1[i] * u; + } + // Last iteration with i == n+1 + end_n *= end; + u = u * start + end_n; + sum2 += sm->c2[n-1] * u; + + *s0 = sum0; + *s1 = sum1; + *s2 = sum2; } // Integrate (pos + start_v*t + half_accel*t^2) with smoothing weight function // w(t0 - t) over the range [start; end] -inline double -integrate_weighted(const struct smoother *sm, double pos - , double start_v, double half_accel - , double start, double end, double t0) +__always_inline double +integrate_weighted(const struct move* m, int axis, const struct smoother *sm + , double base, double start, double end, double t0 + , double* smooth_velocity) { + double axis_r = m->axes_r.axis[axis - 'x']; + double start_v = m->start_v * axis_r; + double half_accel = m->half_accel * axis_r; // Substitute the integration variable tnew = t0 - t to simplify integrals - pos += (half_accel * t0 + start_v) * t0; - start_v = -(start_v + 2. * half_accel * t0); - return integrate_nth_order(sm->n, sm->c, t0 - end, t0 - start - , pos, start_v, half_accel); + double accel = 2. * half_accel; + base += (half_accel * t0 + start_v) * t0; + start_v += accel * t0; + if (start <= 0.) start = 0.; + if (end >= m->move_t) end = m->move_t; + double delta_t = end - start; + double s0, s1, s2; + if (unlikely(sm->symm)) + integrate_smoother_symm(sm, t0 - end, t0 - start, &s0, &s1, &s2); + else + integrate_smoother(sm, t0 - end, t0 - start, &s0, &s1, &s2); + double smooth_pos = (base * s0 - start_v * s1 + half_accel * s2) * delta_t; + if (smooth_velocity) + *smooth_velocity = (start_v * s0 - accel * s1) * delta_t; + return smooth_pos; } /**************************************************************** @@ -75,23 +116,33 @@ integrate_weighted(const struct smoother *sm, double pos int init_smoother(int n, const double a[], double t_sm, struct smoother* sm) { - if (n < 0 || n > ARRAY_SIZE(sm->c)) + if (n < 2 || n + 2 > ARRAY_SIZE(sm->c0)) return -1; - memset(sm->c, 0, sizeof(sm->c)); + memset(sm, 0, sizeof(*sm)); sm->n = n; sm->hst = 0.5 * t_sm; if (!t_sm) return 0; double inv_t_sm = 1. / t_sm; double inv_t_sm_n = inv_t_sm; - int i; + int i, symm = n & 1; for (i = 0; i < n; ++i) { - sm->c[i] = a[i] * inv_t_sm_n; + if ((i & 1) && a[i]) symm = 0; + double c = a[i] * inv_t_sm_n; + sm->c0[i] = c * coeffs[i]; + sm->c1[i] = c * coeffs[i+1]; + sm->c2[i] = c * coeffs[i+2]; inv_t_sm_n *= inv_t_sm; } - double inv_norm = 1. / integrate_nth_order(n, sm->c, -sm->hst, sm->hst - , 1., 0., 0.); - for (i = 0; i < n; ++i) - sm->c[i] *= inv_norm; - sm->t_offs = integrate_nth_order(n, sm->c, -sm->hst, sm->hst, 0., 1., 0.); + sm->symm = symm; + double norm, t_int, t2_int; + integrate_smoother(sm, -sm->hst, sm->hst, &norm, &t_int, &t2_int); + double inv_norm = inv_t_sm / norm; + for (i = 0; i < n; ++i) { + sm->c0[i] *= inv_norm; + sm->c1[i] *= inv_norm; + sm->c2[i] *= inv_norm; + } + integrate_smoother(sm, -sm->hst, sm->hst, &norm, &t_int, &t2_int); + sm->t_offs = t_int * t_sm; return 0; } diff --git a/klippy/chelper/integrate.h b/klippy/chelper/integrate.h index 22856979a..ce64cbf15 100644 --- a/klippy/chelper/integrate.h +++ b/klippy/chelper/integrate.h @@ -2,14 +2,17 @@ #define INTEGRATE_H struct smoother { - double c[12]; + double c0[14], c1[14], c2[14]; double hst, t_offs; - int n; + int n, symm; }; +struct move; + int init_smoother(int n, const double a[], double t_sm, struct smoother* sm); -double integrate_weighted(const struct smoother *sm, - double pos, double start_v, double half_accel, - double start, double end, double t0); +double integrate_weighted(const struct move* m, int axis + , const struct smoother *sm, double base + , double start, double end, double t0 + , double* smooth_velocity); #endif // integrate.h diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 62c27fddb..b2888fe26 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -22,11 +22,17 @@ // deceleration). The formula is: // pa_position(t) = (nominal_position(t) // + pressure_advance * nominal_velocity(t)) -// Which is then "smoothed" using a weighted average: +// The nominal position and velocity are then smoothed using a weighted average: // smooth_position(t) = ( -// definitive_integral(pa_position(x + t_offs) * smoother(t-x) * dx, +// definitive_integral(nominal_position(x+t_offs) * smoother(t-x) * dx, // from=t-smooth_time/2, to=t+smooth_time/2) -// / ((smooth_time/2)**2)) +// smooth_velocity(t) = ( +// definitive_integral(nominal_velocity(x+t_offs) * smoother(t-x) * dx, +// from=t-smooth_time/2, to=t+smooth_time/2) +// and the final pressure advance value calculated as +// smooth_pa_position(t) = smooth_position(t) + pa_func(smooth_velocity(t)) +// where pa_func(v) = pressure_advance * v for linear velocity model or a more +// complicated function for non-linear pressure advance models. // Calculate the definitive integral of extruder for a given move static void @@ -35,21 +41,15 @@ pa_move_integrate(const struct move *m, int axis , double t0, const struct smoother *sm , double *pos_integral, double *pa_velocity_integral) { - if (start < 0.) - start = 0.; - if (end > m->move_t) - end = m->move_t; // Calculate base position and velocity with pressure advance int can_pressure_advance = m->axes_r.x > 0. || m->axes_r.y > 0.; - double axis_r = m->axes_r.axis[axis - 'x']; - double start_v = m->start_v * axis_r; - double ha = m->half_accel * axis_r; // Calculate definitive integral + double smooth_velocity; *pos_integral += integrate_weighted( - sm, base, start_v, ha, start, end, t0); + m, axis, sm, base, start, end, t0, + can_pressure_advance ? &smooth_velocity : NULL); if (can_pressure_advance) { - *pa_velocity_integral += integrate_weighted( - sm, start_v, 2. * ha, 0., start, end, t0); + *pa_velocity_integral += smooth_velocity; } } @@ -72,7 +72,7 @@ pa_range_integrate(const struct move *m, int axis, double move_time double start = move_time - sm->hst, end = move_time + sm->hst; double start_base = m->start_pos.axis[axis - 'x']; *pos_integral = *pa_velocity_integral = 0.; - pa_move_integrate(m, axis, 0., start, end, move_time, + pa_move_integrate(m, axis, 0., start, end, /*t0=*/move_time, sm, pos_integral, pa_velocity_integral); // Integrate over previous moves const struct move *prev = m; @@ -81,7 +81,7 @@ pa_range_integrate(const struct move *m, int axis, double move_time start += prev->move_t; double base = prev->start_pos.axis[axis - 'x'] - start_base; pa_move_integrate(prev, axis, base, start, prev->move_t, - start + sm->hst, sm, pos_integral, + /*t0=*/start + sm->hst, sm, pos_integral, pa_velocity_integral); } // Integrate over future moves @@ -89,7 +89,7 @@ pa_range_integrate(const struct move *m, int axis, double move_time end -= m->move_t; m = list_next_entry(m, node); double base = m->start_pos.axis[axis - 'x'] - start_base; - pa_move_integrate(m, axis, base, 0., end, end - sm->hst, + pa_move_integrate(m, axis, base, 0., end, /*t0=*/end - sm->hst, sm, pos_integral, pa_velocity_integral); } *pos_integral += start_base; diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c index 6b9f53e7b..4d00a283a 100644 --- a/klippy/chelper/kin_shaper.c +++ b/klippy/chelper/kin_shaper.c @@ -101,23 +101,6 @@ shaper_calc_position(const struct move *m, int axis, double move_time * Generic position calculation via smoother integration ****************************************************************/ -// Calculate the definitive integral on a part of a move -static double -move_integrate(const struct move *m, int axis, double start, double end - , double t0, const struct smoother *sm) -{ - if (start < 0.) - start = 0.; - if (end > m->move_t) - end = m->move_t; - double axis_r = m->axes_r.axis[axis - 'x']; - double start_pos = m->start_pos.axis[axis - 'x']; - double res = integrate_weighted(sm, start_pos, - axis_r * m->start_v, axis_r * m->half_accel, - start, end, t0); - return res; -} - // Calculate the definitive integral over a range of moves static double range_integrate(const struct move *m, int axis, double move_time @@ -134,20 +117,23 @@ range_integrate(const struct move *m, int axis, double move_time } // Calculate integral for the current move double start = move_time - sm->hst, end = move_time + sm->hst; - double res = move_integrate(m, axis, start, end, /*t0=*/move_time, sm); + double res = integrate_weighted(m, axis, sm, m->start_pos.axis[axis - 'x'], + start, end, /*t0=*/move_time, NULL); // Integrate over previous moves const struct move *prev = m; while (unlikely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; - res += move_integrate(prev, axis, start, prev->move_t, - /*t0=*/start + sm->hst, sm); + res += integrate_weighted( + prev, axis, sm, prev->start_pos.axis[axis - 'x'], + start, prev->move_t, /*t0=*/start + sm->hst, NULL); } // Integrate over future moves while (unlikely(end > m->move_t)) { end -= m->move_t; m = list_next_entry(m, node); - res += move_integrate(m, axis, 0., end, /*t0=*/end - sm->hst, sm); + res += integrate_weighted(m, axis, sm, m->start_pos.axis[axis - 'x'], + 0., end, /*t0=*/end - sm->hst, NULL); } return res; } From 961c22a73f2ca6c6aa8f99cdc9a8988bc03ac7fe Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sat, 6 May 2023 23:50:34 +0200 Subject: [PATCH 22/35] chelper: Added O3, NEON (for ARM) and native CPU optimizations flags This was tested on the RPi3 B+ and shown to improve the performance Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 8e795ce86..333fa8f40 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -12,12 +12,12 @@ ###################################################################### GCC_CMD = "gcc" -COMPILE_ARGS = ( - "-Wall -g -O2 -shared -fPIC" - " -flto -fwhole-program -fno-use-linker-plugin" - " -o %s %s" -) +COMPILE_ARGS = ("-Wall -g -O3 -shared -fPIC" + " -flto -fwhole-program -fno-use-linker-plugin" + " -march=native -mcpu=native -mtune=native" + " -o %s %s") SSE_FLAGS = "-mfpmath=sse -msse2" +NEON_FLAGS = "-mfpu=neon" SOURCE_FILES = [ 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'stepcompress_hp.c', 'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c', @@ -315,6 +315,8 @@ def get_ffi(): if check_build_code(srcfiles + ofiles + [__file__], destlib): if check_gcc_option(SSE_FLAGS): cmd = "%s %s %s" % (GCC_CMD, SSE_FLAGS, COMPILE_ARGS) + elif check_gcc_option(NEON_FLAGS): + cmd = "%s %s %s" % (GCC_CMD, NEON_FLAGS, COMPILE_ARGS) else: cmd = "%s %s" % (GCC_CMD, COMPILE_ARGS) logging.info("Building C code module %s", DEST_LIB) From dfdabe0ffbfa0dc9734ac1ef098dbf05c33f1b59 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 10 May 2023 22:12:19 +0200 Subject: [PATCH 23/35] input_shaper: Updated and added some smoother definitions Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_defs.py | 58 ++++++++++++------------------------ 1 file changed, 19 insertions(+), 39 deletions(-) diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index fe172de79..0e27266b2 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -145,19 +145,19 @@ def get_zv_smoother(shaper_freq, damping_ratio_unused=None, -1.465471373781904,0.01966833207740377] return init_smoother(coeffs, 0.8025 / shaper_freq, normalize_coeffs) -def get_mzv20_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-1928.625991372561,174.47238885576,680.8819871456237, - -54.08725118644957,-55.28572183696386,2.617288493127363, - 1.401087377835678] - return init_smoother(coeffs, 0.85175 / shaper_freq, normalize_coeffs) - -def get_mzv16_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-1938.774071514084,157.7065914461193,711.7324202087348, - -48.34288782208795,-63.72538103023892,2.22905999013954, - 1.741413861921329] - return init_smoother(coeffs, 0.9665 / shaper_freq, normalize_coeffs) +def get_mzv_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-1906.717580206364,125.8892756660212,698.0200035767849, + -37.75923018121473,-62.18762409216703,1.57172781617736, + 1.713117990217123] + return init_smoother(coeffs, 0.95625 / shaper_freq, normalize_coeffs) + +def get_ei_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-1797.048868963208,120.5310596109878,669.6653197989012, + -35.71975707450795,-62.49388325512682,1.396748042940248, + 1.848276903900512] + return init_smoother(coeffs, 1.06625 / shaper_freq, normalize_coeffs) def get_2hump_ei_smoother(shaper_freq, damping_ratio_unused=None, normalize_coeffs=True): @@ -168,28 +168,10 @@ def get_2hump_ei_smoother(shaper_freq, damping_ratio_unused=None, def get_si_smoother(shaper_freq, damping_ratio_unused=None, normalize_coeffs=True): - coeffs = [-121313.2032178889,26921.56924860698,63029.77630233325, - -13293.49355097946,-10648.58182242004,2050.103214332665, - 666.126576474768,-102.9883580154977,-17.0691108398123, - 0.1640954647660932,1.278318992073449] - return init_smoother(coeffs, 1.2 / shaper_freq, normalize_coeffs) - -def get_nei_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [4883445.221306241,-720494.0751766556,-3148819.580240736, - 424777.3712490448,721441.1426576816,-85447.8333563426, - -70054.38903665643,6764.372246231036,2669.737943114452, - -180.6374244922258,-30.4660136303673,1.82937577459115] - return init_smoother(coeffs, 1.42 / shaper_freq, normalize_coeffs) - -def get_nsi_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [1.576037300103821e7,-3450496.635175148,-1.138914512941258e7, - 2263562.40616947,3103579.174607452,-537674.9863778015, - -397766.323762382,56331.41234254206,24270.29707072854, - -2532.279663870225,-598.7901101230622,36.92208279328751, - -0.9886556775166597,1.047798776818597] - return init_smoother(coeffs, 1.58 / shaper_freq, normalize_coeffs) + coeffs = [-6186.76006449789,1206.747198930197,2579.985143622855, + -476.8554763069169,-295.546608490564,52.69679971161049, + 4.234582468800491,-2.226157642004671,1.267781046297883] + return init_smoother(coeffs, 1.245 / shaper_freq, normalize_coeffs) # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ @@ -203,10 +185,8 @@ def get_nsi_smoother(shaper_freq, damping_ratio_unused=None, INPUT_SMOOTHERS = [ InputSmootherCfg('smooth_zv', get_zv_smoother, min_freq=21.), - InputSmootherCfg('smooth_mzv20', get_mzv20_smoother, min_freq=23.), - InputSmootherCfg('smooth_mzv16', get_mzv16_smoother, min_freq=23.), + InputSmootherCfg('smooth_mzv', get_mzv_smoother, min_freq=23.), + InputSmootherCfg('smooth_ei', get_ei_smoother, min_freq=23.), InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=23.), InputSmootherCfg('smooth_si', get_si_smoother, min_freq=23.), - InputSmootherCfg('smooth_nei', get_nei_smoother, min_freq=23.), - InputSmootherCfg('smooth_nsi', get_nsi_smoother, min_freq=23.), ] From 69437926a47e61851030f235253a690e5fe2767c Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 10 May 2023 22:08:43 +0200 Subject: [PATCH 24/35] integrate: Faster integration via antiderivatives calculation Signed-off-by: Dmitry Butyugin --- klippy/chelper/integrate.c | 114 +++++++++++----------------------- klippy/chelper/integrate.h | 21 +++++-- klippy/chelper/kin_extruder.c | 57 ++++++++++++----- klippy/chelper/kin_shaper.c | 39 +++++++++--- 4 files changed, 122 insertions(+), 109 deletions(-) diff --git a/klippy/chelper/integrate.c b/klippy/chelper/integrate.c index 91eec75db..e34c5e983 100644 --- a/klippy/chelper/integrate.c +++ b/klippy/chelper/integrate.c @@ -20,73 +20,38 @@ static double coeffs[] = { 1./11., 1./12., 1./13., 1./14., 1./15., 1./16., 1./17., 1./18., 1./19., }; -inline static void -integrate_smoother(const struct smoother* sm, double start, double end - , double* s0, double* s1, double* s2) +inline smoother_antiderivatives +calc_antiderivatives(const struct smoother* sm, double t) { - int n = sm->n; - double u = start + end; - double end_n = end; - // First 2 iterations with i == 0 and 1 - double sum0 = sm->c0[0] + sm->c0[1] * u, sum1 = sm->c1[0] * u, sum2 = 0.; - int i; + int n = sm->n, i; + double it0 = (sm->c0[0] * t + sm->c0[1]) * t; + double it1 = (sm->c1[0] * t + sm->c1[1]) * t; + double it2 = (sm->c2[0] * t + sm->c2[1]) * t; for (i = 2; i < n; ++i) { - end_n *= end; - u = u * start + end_n; - sum2 += sm->c2[i-2] * u; - sum1 += sm->c1[i-1] * u; - sum0 += sm->c0[i] * u; + it0 = (it0 + sm->c0[i]) * t; + it1 = (it1 + sm->c1[i]) * t; + it2 = (it2 + sm->c2[i]) * t; } - // Remainging 2 iterations with i == n and n+1 - end_n *= end; - u = u * start + end_n; - sum2 += sm->c2[n-2] * u; - sum1 += sm->c1[n-1] * u; - - end_n *= end; - u = u * start + end_n; - sum2 += sm->c2[n-1] * u; - - *s0 = sum0; - *s1 = sum1; - *s2 = sum2; + it1 *= t; + it2 *= t * t; + return (smoother_antiderivatives) { + .it0 = it0, .it1 = it1, .it2 = it2 }; } -inline static void -integrate_smoother_symm(const struct smoother* sm, double start, double end - , double* s0, double* s1, double* s2) +inline smoother_antiderivatives +diff_antiderivatives(const smoother_antiderivatives* ad1 + , const smoother_antiderivatives* ad2) { - int n = sm->n; - double u = start + end; - double end_n = end; - // First 2 iterations with i == 0 and 1 - double sum0 = sm->c0[0], sum1 = sm->c1[0] * u, sum2 = 0.; - int i; - for (i = 2; i < n; i += 2) { - end_n *= end; - u = u * start + end_n; - sum2 += sm->c2[i-2] * u; - sum0 += sm->c0[i] * u; - end_n *= end; - u = u * start + end_n; - sum1 += sm->c1[i] * u; - } - // Last iteration with i == n+1 - end_n *= end; - u = u * start + end_n; - sum2 += sm->c2[n-1] * u; - - *s0 = sum0; - *s1 = sum1; - *s2 = sum2; + return (smoother_antiderivatives) { + .it0 = ad2->it0 - ad1->it0, + .it1 = ad2->it1 - ad1->it1, + .it2 = ad2->it2 - ad1->it2 }; } -// Integrate (pos + start_v*t + half_accel*t^2) with smoothing weight function -// w(t0 - t) over the range [start; end] -__always_inline double -integrate_weighted(const struct move* m, int axis, const struct smoother *sm - , double base, double start, double end, double t0 - , double* smooth_velocity) +inline double +integrate_move(const struct move* m, int axis, double base, double t0 + , const smoother_antiderivatives* s + , double* smooth_velocity) { double axis_r = m->axes_r.axis[axis - 'x']; double start_v = m->start_v * axis_r; @@ -95,17 +60,9 @@ integrate_weighted(const struct move* m, int axis, const struct smoother *sm double accel = 2. * half_accel; base += (half_accel * t0 + start_v) * t0; start_v += accel * t0; - if (start <= 0.) start = 0.; - if (end >= m->move_t) end = m->move_t; - double delta_t = end - start; - double s0, s1, s2; - if (unlikely(sm->symm)) - integrate_smoother_symm(sm, t0 - end, t0 - start, &s0, &s1, &s2); - else - integrate_smoother(sm, t0 - end, t0 - start, &s0, &s1, &s2); - double smooth_pos = (base * s0 - start_v * s1 + half_accel * s2) * delta_t; + double smooth_pos = base * s->it0 - start_v * s->it1 + half_accel * s->it2; if (smooth_velocity) - *smooth_velocity = (start_v * s0 - accel * s1) * delta_t; + *smooth_velocity = start_v * s->it0 - accel * s->it1; return smooth_pos; } @@ -116,7 +73,7 @@ integrate_weighted(const struct move* m, int axis, const struct smoother *sm int init_smoother(int n, const double a[], double t_sm, struct smoother* sm) { - if (n < 2 || n + 2 > ARRAY_SIZE(sm->c0)) + if ((t_sm && n < 2) || n > ARRAY_SIZE(sm->c0)) return -1; memset(sm, 0, sizeof(*sm)); sm->n = n; @@ -128,21 +85,22 @@ init_smoother(int n, const double a[], double t_sm, struct smoother* sm) for (i = 0; i < n; ++i) { if ((i & 1) && a[i]) symm = 0; double c = a[i] * inv_t_sm_n; - sm->c0[i] = c * coeffs[i]; - sm->c1[i] = c * coeffs[i+1]; - sm->c2[i] = c * coeffs[i+2]; + sm->c0[n-1-i] = c * coeffs[i]; + sm->c1[n-1-i] = c * coeffs[i+1]; + sm->c2[n-1-i] = c * coeffs[i+2]; inv_t_sm_n *= inv_t_sm; } sm->symm = symm; - double norm, t_int, t2_int; - integrate_smoother(sm, -sm->hst, sm->hst, &norm, &t_int, &t2_int); - double inv_norm = inv_t_sm / norm; + double inv_norm = 1. / (calc_antiderivatives(sm, sm->hst).it0 + - calc_antiderivatives(sm, -sm->hst).it0); for (i = 0; i < n; ++i) { sm->c0[i] *= inv_norm; sm->c1[i] *= inv_norm; sm->c2[i] *= inv_norm; } - integrate_smoother(sm, -sm->hst, sm->hst, &norm, &t_int, &t2_int); - sm->t_offs = t_int * t_sm; + sm->p_hst = calc_antiderivatives(sm, sm->hst); + sm->m_hst = calc_antiderivatives(sm, -sm->hst); + sm->pm_diff = diff_antiderivatives(&sm->m_hst, &sm->p_hst); + sm->t_offs = sm->pm_diff.it1; return 0; } diff --git a/klippy/chelper/integrate.h b/klippy/chelper/integrate.h index ce64cbf15..620d7c975 100644 --- a/klippy/chelper/integrate.h +++ b/klippy/chelper/integrate.h @@ -1,18 +1,29 @@ #ifndef INTEGRATE_H #define INTEGRATE_H +typedef struct { + double it0, it1, it2; +} smoother_antiderivatives; + struct smoother { - double c0[14], c1[14], c2[14]; + double c0[10], c1[10], c2[10]; double hst, t_offs; + smoother_antiderivatives m_hst, p_hst, pm_diff; int n, symm; }; struct move; int init_smoother(int n, const double a[], double t_sm, struct smoother* sm); -double integrate_weighted(const struct move* m, int axis - , const struct smoother *sm, double base - , double start, double end, double t0 - , double* smooth_velocity); + +double integrate_move(const struct move* m, int axis, double base, double t0 + , const smoother_antiderivatives* s + , double* smooth_velocity); + +smoother_antiderivatives +calc_antiderivatives(const struct smoother* sm, double t); +smoother_antiderivatives +diff_antiderivatives(const smoother_antiderivatives* ad1 + , const smoother_antiderivatives* ad2); #endif // integrate.h diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index b2888fe26..06881c5cf 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -35,19 +35,18 @@ // complicated function for non-linear pressure advance models. // Calculate the definitive integral of extruder for a given move -static void -pa_move_integrate(const struct move *m, int axis - , double base, double start, double end - , double t0, const struct smoother *sm +static inline void +pa_move_integrate(const struct move *m, int axis, double base + , double t0, const smoother_antiderivatives *ad , double *pos_integral, double *pa_velocity_integral) { // Calculate base position and velocity with pressure advance int can_pressure_advance = m->axes_r.x > 0. || m->axes_r.y > 0.; - // Calculate definitive integral double smooth_velocity; - *pos_integral += integrate_weighted( - m, axis, sm, base, start, end, t0, - can_pressure_advance ? &smooth_velocity : NULL); + // Calculate definitive integral + *pos_integral += integrate_move(m, axis, base, t0, ad, + can_pressure_advance ? &smooth_velocity + : NULL); if (can_pressure_advance) { *pa_velocity_integral += smooth_velocity; } @@ -70,27 +69,51 @@ pa_range_integrate(const struct move *m, int axis, double move_time } // Calculate integral for the current move double start = move_time - sm->hst, end = move_time + sm->hst; + double t0 = move_time; double start_base = m->start_pos.axis[axis - 'x']; *pos_integral = *pa_velocity_integral = 0.; - pa_move_integrate(m, axis, 0., start, end, /*t0=*/move_time, - sm, pos_integral, pa_velocity_integral); + if (unlikely(start >= 0. && end <= m->move_t)) { + pa_move_integrate(m, axis, 0., t0, &sm->pm_diff, + pos_integral, pa_velocity_integral); + *pos_integral += start_base; + return; + } + smoother_antiderivatives left = + likely(start < 0.) ? calc_antiderivatives(sm, t0) : sm->p_hst; + smoother_antiderivatives right = + likely(end > m->move_t) ? calc_antiderivatives(sm, t0 - m->move_t) + : sm->m_hst; + smoother_antiderivatives diff = diff_antiderivatives(&right, &left); + pa_move_integrate(m, axis, 0., t0, &diff, + pos_integral, pa_velocity_integral); // Integrate over previous moves const struct move *prev = m; - while (unlikely(start < 0.)) { + while (likely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; + t0 += prev->move_t; + smoother_antiderivatives r = left; + left = likely(start < 0.) ? calc_antiderivatives(sm, t0) + : sm->p_hst; + diff = diff_antiderivatives(&r, &left); double base = prev->start_pos.axis[axis - 'x'] - start_base; - pa_move_integrate(prev, axis, base, start, prev->move_t, - /*t0=*/start + sm->hst, sm, pos_integral, - pa_velocity_integral); + pa_move_integrate(prev, axis, base, t0, &diff, + pos_integral, pa_velocity_integral); } // Integrate over future moves - while (unlikely(end > m->move_t)) { + t0 = move_time; + while (likely(end > m->move_t)) { end -= m->move_t; + t0 -= m->move_t; m = list_next_entry(m, node); + smoother_antiderivatives l = right; + right = likely(end > m->move_t) ? calc_antiderivatives(sm, + t0 - m->move_t) + : sm->m_hst; + diff = diff_antiderivatives(&right, &l); double base = m->start_pos.axis[axis - 'x'] - start_base; - pa_move_integrate(m, axis, base, 0., end, /*t0=*/end - sm->hst, - sm, pos_integral, pa_velocity_integral); + pa_move_integrate(m, axis, base, t0, &diff, + pos_integral, pa_velocity_integral); } *pos_integral += start_base; } diff --git a/klippy/chelper/kin_shaper.c b/klippy/chelper/kin_shaper.c index 4d00a283a..c368fa36f 100644 --- a/klippy/chelper/kin_shaper.c +++ b/klippy/chelper/kin_shaper.c @@ -117,23 +117,44 @@ range_integrate(const struct move *m, int axis, double move_time } // Calculate integral for the current move double start = move_time - sm->hst, end = move_time + sm->hst; - double res = integrate_weighted(m, axis, sm, m->start_pos.axis[axis - 'x'], - start, end, /*t0=*/move_time, NULL); + double t0 = move_time; + if (unlikely(start >= 0. && end <= m->move_t)) + return integrate_move(m, axis, m->start_pos.axis[axis - 'x'], + t0, &sm->pm_diff, NULL); + smoother_antiderivatives left = + likely(start < 0.) ? calc_antiderivatives(sm, t0) : sm->p_hst; + smoother_antiderivatives right = + likely(end > m->move_t) ? calc_antiderivatives(sm, t0 - m->move_t) + : sm->m_hst; + smoother_antiderivatives diff = diff_antiderivatives(&right, &left); + double res = integrate_move(m, axis, m->start_pos.axis[axis - 'x'], + t0, &diff, NULL); // Integrate over previous moves const struct move *prev = m; - while (unlikely(start < 0.)) { + while (likely(start < 0.)) { prev = list_prev_entry(prev, node); start += prev->move_t; - res += integrate_weighted( - prev, axis, sm, prev->start_pos.axis[axis - 'x'], - start, prev->move_t, /*t0=*/start + sm->hst, NULL); + t0 += prev->move_t; + smoother_antiderivatives r = left; + left = likely(start < 0.) ? calc_antiderivatives(sm, t0) + : sm->p_hst; + diff = diff_antiderivatives(&r, &left); + res += integrate_move(prev, axis, prev->start_pos.axis[axis - 'x'], + t0, &diff, NULL); } // Integrate over future moves - while (unlikely(end > m->move_t)) { + t0 = move_time; + while (likely(end > m->move_t)) { end -= m->move_t; + t0 -= m->move_t; m = list_next_entry(m, node); - res += integrate_weighted(m, axis, sm, m->start_pos.axis[axis - 'x'], - 0., end, /*t0=*/end - sm->hst, NULL); + smoother_antiderivatives l = right; + right = likely(end > m->move_t) ? calc_antiderivatives(sm, + t0 - m->move_t) + : sm->m_hst; + diff = diff_antiderivatives(&right, &l); + res += integrate_move(m, axis, m->start_pos.axis[axis - 'x'], + t0, &diff, NULL); } return res; } From c0aa6688959f73121784678a236e5676dc4c677c Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Mon, 22 May 2023 00:03:15 +0200 Subject: [PATCH 25/35] scripts: Support smoothers in shaper calibration and plotting scripts Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_calibrate.py | 170 ++++++++++----- scripts/graph_shaper.py | 350 +++++++++++++----------------- 2 files changed, 262 insertions(+), 258 deletions(-) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index 8bd03d3d7..c9c625661 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -18,7 +18,8 @@ TEST_DAMPING_RATIOS = [0.075, 0.1, 0.15] -AUTOTUNE_SHAPERS = ["zv", "mzv", "ei", "2hump_ei", "3hump_ei"] +AUTOTUNE_SHAPERS = ['mzv', 'ei', '2hump_ei', 'smooth_zv', 'smooth_mzv', + 'smooth_ei', 'smooth_2hump_ei', 'smooth_si'] ###################################################################### # Frequency response calculation and shaper auto-tuning @@ -74,6 +75,53 @@ def get_psd(self, axis="all"): ) +def get_shaper_offset(A, T): + return sum([a * t for a, t in zip(A, T)]) / sum(A) + +def get_smoother_offset(np, C, t_sm): + hst = t_sm * 0.5 + + n_t = 1000 + t, dt = np.linspace(-hst, hst, n_t, retstep=True) + + w = np.zeros(shape=t.shape) + for c in C[::-1]: + w = w * (-t) + c + + return -np.trapz(t * w, dx=dt) + +def estimate_shaper(np, shaper, test_damping_ratio, test_freqs): + A, T = np.asarray(shaper[0]), np.asarray(shaper[1]) + inv_D = 1. / A.sum() + + omega = 2. * math.pi * np.asarray(test_freqs) + damping = test_damping_ratio * omega + omega_d = omega * math.sqrt(1. - test_damping_ratio**2) + W = A * np.exp(np.outer(-damping, (T[-1] - T))) + S = W * np.sin(np.outer(omega_d, T)) + C = W * np.cos(np.outer(omega_d, T)) + return np.sqrt(S.sum(axis=1)**2 + C.sum(axis=1)**2) * inv_D + +def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): + C, t_sm = smoother[0], smoother[1] + hst = t_sm * 0.5 + + test_freqs = np.asarray(test_freqs) + omega = 2. * math.pi * test_freqs + damping = test_damping_ratio * omega + omega_d = omega * math.sqrt(1. - test_damping_ratio**2) + + n_t = max(100, 100 * round(t_sm * np.max(test_freqs))) + t, dt = np.linspace(0., t_sm, n_t, retstep=True) + w = np.zeros(shape=t.shape) + for c in C[::-1]: + w = w * (t-hst) + c + + E = w * np.exp(np.outer(damping, (t-t_sm))) + C = np.cos(np.outer(omega_d, (t-t_sm))) + S = np.sin(np.outer(omega_d, (t-t_sm))) + return np.sqrt(np.trapz(E * C, dx=dt)**2 + np.trapz(E * S, dx=dt)**2) + class ShaperCalibrate: def __init__(self, printer): self.printer = printer @@ -205,24 +253,7 @@ def process_accelerometer_data(self, data): calibration_data.set_numpy(self.numpy) return calibration_data - def _estimate_shaper(self, shaper, test_damping_ratio, test_freqs): - np = self.numpy - - A, T = np.array(shaper[0]), np.array(shaper[1]) - inv_D = 1.0 / A.sum() - - omega = 2.0 * math.pi * test_freqs - damping = test_damping_ratio * omega - omega_d = omega * math.sqrt(1.0 - test_damping_ratio**2) - W = A * np.exp(np.outer(-damping, (T[-1] - T))) - S = W * np.sin(np.outer(omega_d, T)) - C = W * np.cos(np.outer(omega_d, T)) - return np.sqrt(S.sum(axis=1) ** 2 + C.sum(axis=1) ** 2) * inv_D - - def _estimate_remaining_vibrations( - self, shaper, test_damping_ratio, freq_bins, psd - ): - vals = self._estimate_shaper(shaper, test_damping_ratio, freq_bins) + def _estimate_remaining_vibrations(self, vals, psd): # The input shaper can only reduce the amplitude of vibrations by # SHAPER_VIBRATION_REDUCTION times, so all vibrations below that # threshold can be igonred @@ -231,7 +262,7 @@ def _estimate_remaining_vibrations( vals * psd - vibr_threshold, 0 ).sum() all_vibrations = self.numpy.maximum(psd - vibr_threshold, 0).sum() - return (remaining_vibrations / all_vibrations, vals) + return remaining_vibrations / all_vibrations def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): half_accel = accel * 0.5 @@ -239,23 +270,41 @@ def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): A, T = shaper inv_D = 1.0 / sum(A) n = len(T) - # Calculate input shaper shift - ts = sum([A[i] * T[i] for i in range(n)]) * inv_D + ts = get_shaper_offset(A, T) # Calculate offset for 90 and 180 degrees turn - offset_90 = offset_180 = 0.0 + offset_90_x = offset_90_y = offset_180 = 0. for i in range(n): if T[i] >= ts: # Calculate offset for one of the axes - offset_90 += ( - A[i] * (scv + half_accel * (T[i] - ts)) * (T[i] - ts) - ) - offset_180 += A[i] * half_accel * (T[i] - ts) ** 2 - offset_90 *= inv_D * math.sqrt(2.0) + offset_90_x += A[i] * (scv + half_accel * (T[i]-ts)) * (T[i]-ts) + else: + offset_90_y += A[i] * (scv - half_accel * (T[i]-ts)) * (T[i]-ts) + offset_180 += A[i] * half_accel * (T[i]-ts)**2 + offset_90 = inv_D * math.sqrt(offset_90_x**2 + offset_90_y**2) offset_180 *= inv_D - return max(offset_90, offset_180) + return max(offset_90, abs(offset_180)) - def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing): + def _get_smoother_smoothing(self, smoother, accel=5000, scv=5.): + np = self.numpy + half_accel = accel * .5 + + C, t_sm = smoother + hst = 0.5 * t_sm + t, dt = np.linspace(-hst, hst, 100, retstep=True) + w = np.zeros(shape=t.shape) + for c in C[::-1]: + w = w * (-t) + c + t += get_smoother_offset(np, C, t_sm) + + offset_180 = np.trapz(half_accel * t**2 * w, dx=dt) + offset_90_x = np.trapz(((scv + half_accel * t) * t * w)[t >= 0], dx=dt) + offset_90_y = np.trapz(((scv - half_accel * t) * t * w)[t < 0], dx=dt) + offset_90 = math.sqrt(offset_90_x**2 + offset_90_y**2) + return max(offset_90, abs(offset_180)) + + def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, + estimate_shaper, get_shaper_smoothing): np = self.numpy test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, 0.2) @@ -270,21 +319,19 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing): shaper_vibrations = 0.0 shaper_vals = np.zeros(shape=freq_bins.shape) shaper = shaper_cfg.init_func( - test_freq, shaper_defs.DEFAULT_DAMPING_RATIO - ) - shaper_smoothing = self._get_shaper_smoothing(shaper) + test_freq, shaper_defs.DEFAULT_DAMPING_RATIO) + shaper_smoothing = get_shaper_smoothing(shaper) if max_smoothing and shaper_smoothing > max_smoothing and best_res: return best_res # Exact damping ratio of the printer is unknown, pessimizing # remaining vibrations over possible damping values for dr in TEST_DAMPING_RATIOS: - vibrations, vals = self._estimate_remaining_vibrations( - shaper, dr, freq_bins, psd - ) + vals = estimate_shaper(self.numpy, shaper, dr, freq_bins) + vibrations = self._estimate_remaining_vibrations(vals, psd) shaper_vals = np.maximum(shaper_vals, vals) if vibrations > shaper_vibrations: shaper_vibrations = vibrations - max_accel = self.find_shaper_max_accel(shaper) + max_accel = self.find_max_accel(shaper, get_shaper_smoothing) # The score trying to minimize vibrations, but also accounting # the growth of smoothing. The formula itself does not have any # special meaning, it simply shows good results on real user data @@ -309,7 +356,7 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing): # much worse than the 'best' one, but gives much less smoothing selected = best_res for res in results[::-1]: - if res.vibrs < best_res.vibrs * 1.1 and res.score < selected.score: + if res.vibrs < best_res.vibrs * 1.01 and res.score < selected.score: selected = res return selected @@ -329,25 +376,45 @@ def _bisect(self, func): right = middle return left - def find_shaper_max_accel(self, shaper): + def find_max_accel(self, s, get_smoothing): # Just some empirically chosen value which produces good projections # for max_accel without much smoothing TARGET_SMOOTHING = 0.12 - max_accel = self._bisect( - lambda test_accel: self._get_shaper_smoothing(shaper, test_accel) - <= TARGET_SMOOTHING - ) + max_accel = self._bisect(lambda test_accel: get_smoothing( + s, test_accel) <= TARGET_SMOOTHING) return max_accel def find_best_shaper(self, calibration_data, max_smoothing, logger=None): best_shaper = None all_shapers = [] + for smoother_cfg in shaper_defs.INPUT_SMOOTHERS: + if smoother_cfg.name not in AUTOTUNE_SHAPERS: + continue + smoother = self.background_process_exec(self.fit_shaper, ( + smoother_cfg, calibration_data, max_smoothing, + estimate_smoother, self._get_smoother_smoothing)) + if logger is not None: + logger("Fitted smoother '%s' frequency = %.1f Hz " + "(vibrations = %.1f%%, smoothing ~= %.3f)" % ( + smoother.name, smoother.freq, smoother.vibrs * 100., + smoother.smoothing)) + logger("To avoid too much smoothing with '%s', suggested " + "max_accel <= %.0f mm/sec^2" % ( + smoother.name, + round(smoother.max_accel / 100.) * 100.)) + all_shapers.append(smoother) + if (best_shaper is None or smoother.score * 1.2 < best_shaper.score + or (smoother.score * 1.03 < best_shaper.score and + smoother.smoothing * 1.01 < best_shaper.smoothing)): + # Either the smoother significantly improves the score (by 20%), + # or it improves the score and smoothing (by 5% and 10% resp.) + best_shaper = smoother for shaper_cfg in shaper_defs.INPUT_SHAPERS: if shaper_cfg.name not in AUTOTUNE_SHAPERS: continue - shaper = self.background_process_exec( - self.fit_shaper, (shaper_cfg, calibration_data, max_smoothing) - ) + shaper = self.background_process_exec(self.fit_shaper, ( + shaper_cfg, calibration_data, max_smoothing, + estimate_shaper, self._get_shaper_smoothing)) if logger is not None: logger( "Fitted shaper '%s' frequency = %.1f Hz " @@ -365,14 +432,9 @@ def find_best_shaper(self, calibration_data, max_smoothing, logger=None): % (shaper.name, round(shaper.max_accel / 100.0) * 100.0) ) all_shapers.append(shaper) - if ( - best_shaper is None - or shaper.score * 1.2 < best_shaper.score - or ( - shaper.score * 1.05 < best_shaper.score - and shaper.smoothing * 1.1 < best_shaper.smoothing - ) - ): + if (best_shaper is None or shaper.score * 1.2 < best_shaper.score or + (shaper.score * 1.03 < best_shaper.score and + shaper.smoothing * 1.01 < best_shaper.smoothing)): # Either the shaper significantly improves the score (by 20%), # or it improves the score and smoothing (by 5% and 10% resp.) best_shaper = shaper diff --git a/scripts/graph_shaper.py b/scripts/graph_shaper.py index 2498d8667..7536504db 100755 --- a/scripts/graph_shaper.py +++ b/scripts/graph_shaper.py @@ -2,14 +2,18 @@ # Script to plot input shapers # # Copyright (C) 2020 Kevin O'Connor -# Copyright (C) 2020 Dmitry Butyugin +# Copyright (C) 2020-2023 Dmitry Butyugin # # This file may be distributed under the terms of the GNU GPLv3 license. -import optparse, math -import matplotlib +import optparse, importlib, math, os, sys +import numpy as np, matplotlib +sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), + '..', 'klippy')) +shaper_defs = importlib.import_module('.shaper_defs', 'extras') +shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras') # A set of damping ratios to calculate shaper response for -DAMPING_RATIOS = [0.05, 0.1, 0.2] +DAMPING_RATIOS=[0.075, 0.1, 0.15] # Parameters of the input shaper SHAPER_FREQ = 50.0 @@ -17,131 +21,14 @@ # Simulate input shaping of step function for these true resonance frequency # and damping ratio -STEP_SIMULATION_RESONANCE_FREQ = 60.0 -STEP_SIMULATION_DAMPING_RATIO = 0.15 +STEP_SIMULATION_RESONANCE_FREQ=50. +STEP_SIMULATION_DAMPING_RATIO=0.1 -# If set, defines which range of frequencies to plot shaper frequency responce +# If set, defines which range of frequencies to plot shaper frequency response PLOT_FREQ_RANGE = [] # If empty, will be automatically determined # PLOT_FREQ_RANGE = [10., 100.] -PLOT_FREQ_STEP = 0.01 - -###################################################################### -# Input shapers -###################################################################### - - -def get_zv_shaper(): - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - A = [1.0, K] - T = [0.0, 0.5 * t_d] - return (A, T, "ZV") - - -def get_zvd_shaper(): - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - A = [1.0, 2.0 * K, K**2] - T = [0.0, 0.5 * t_d, t_d] - return (A, T, "ZVD") - - -def get_mzv_shaper(): - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-0.75 * SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - - a1 = 1.0 - 1.0 / math.sqrt(2.0) - a2 = (math.sqrt(2.0) - 1.0) * K - a3 = a1 * K * K - - A = [a1, a2, a3] - T = [0.0, 0.375 * t_d, 0.75 * t_d] - return (A, T, "MZV") - - -def get_ei_shaper(): - v_tol = 0.05 # vibration tolerance - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - - a1 = 0.25 * (1.0 + v_tol) - a2 = 0.5 * (1.0 - v_tol) * K - a3 = a1 * K * K - - A = [a1, a2, a3] - T = [0.0, 0.5 * t_d, t_d] - return (A, T, "EI") - - -def get_2hump_ei_shaper(): - v_tol = 0.05 # vibration tolerance - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - - V2 = v_tol**2 - X = pow(V2 * (math.sqrt(1.0 - V2) + 1.0), 1.0 / 3.0) - a1 = (3.0 * X * X + 2.0 * X + 3.0 * V2) / (16.0 * X) - a2 = (0.5 - a1) * K - a3 = a2 * K - a4 = a1 * K * K * K - - A = [a1, a2, a3, a4] - T = [0.0, 0.5 * t_d, t_d, 1.5 * t_d] - return (A, T, "2-hump EI") - - -def get_3hump_ei_shaper(): - v_tol = 0.05 # vibration tolerance - df = math.sqrt(1.0 - SHAPER_DAMPING_RATIO**2) - K = math.exp(-SHAPER_DAMPING_RATIO * math.pi / df) - t_d = 1.0 / (SHAPER_FREQ * df) - - K2 = K * K - a1 = 0.0625 * ( - 1.0 + 3.0 * v_tol + 2.0 * math.sqrt(2.0 * (v_tol + 1.0) * v_tol) - ) - a2 = 0.25 * (1.0 - v_tol) * K - a3 = (0.5 * (1.0 + v_tol) - 2.0 * a1) * K2 - a4 = a2 * K2 - a5 = a1 * K2 * K2 - - A = [a1, a2, a3, a4, a5] - T = [0.0, 0.5 * t_d, t_d, 1.5 * t_d, 2.0 * t_d] - return (A, T, "3-hump EI") - - -def estimate_shaper(shaper, freq, damping_ratio): - A, T, _ = shaper - n = len(T) - inv_D = 1.0 / sum(A) - omega = 2.0 * math.pi * freq - damping = damping_ratio * omega - omega_d = omega * math.sqrt(1.0 - damping_ratio**2) - S = C = 0 - for i in range(n): - W = A[i] * math.exp(-damping * (T[-1] - T[i])) - S += W * math.sin(omega_d * T[i]) - C += W * math.cos(omega_d * T[i]) - return math.sqrt(S * S + C * C) * inv_D - - -def shift_pulses(shaper): - A, T, name = shaper - n = len(T) - ts = sum([A[i] * T[i] for i in range(n)]) / sum(A) - for i in range(n): - T[i] -= ts - - -# Shaper selection -get_shaper = get_ei_shaper - +PLOT_FREQ_STEP = .05 ###################################################################### # Plotting and startup @@ -160,92 +47,139 @@ def bisect(func, left, right): return 0.5 * (left + right) -def find_shaper_plot_range(shaper, vib_tol): +def find_shaper_plot_range(estimate_shaper, shaper_freq, vib_tol): def eval_shaper(freq): - return estimate_shaper(shaper, freq, DAMPING_RATIOS[0]) - vib_tol - + return estimate_shaper(freq) - vib_tol if not PLOT_FREQ_RANGE: - left = bisect(eval_shaper, 0.0, SHAPER_FREQ) - right = bisect(eval_shaper, SHAPER_FREQ, 2.4 * SHAPER_FREQ) + left = bisect(eval_shaper, 0.1 * shaper_freq, shaper_freq) + right = bisect(eval_shaper, shaper_freq, 3 * shaper_freq) else: left, right = PLOT_FREQ_RANGE return (left, right) - -def gen_shaper_response(shaper): - # Calculate shaper vibration responce on a range of requencies - response = [] - freqs = [] - freq, freq_end = find_shaper_plot_range(shaper, vib_tol=0.25) - while freq <= freq_end: - vals = [] - for damping_ratio in DAMPING_RATIOS: - vals.append(estimate_shaper(shaper, freq, damping_ratio)) - response.append(vals) - freqs.append(freq) - freq += PLOT_FREQ_STEP - legend = ["damping ratio = %.3f" % d_r for d_r in DAMPING_RATIOS] +def gen_shaper_response(shaper, shaper_freq, estimate_shaper): + # Calculate shaper vibration response on a range of requencies + freq_start, freq_end = find_shaper_plot_range( + lambda freq: estimate_shaper( + np, shaper, DAMPING_RATIOS[0], [freq]), + shaper_freq, vib_tol=0.25) + freqs = np.arange(freq_start, freq_end, PLOT_FREQ_STEP) + response = np.zeros(shape=(freqs.shape[0], len(DAMPING_RATIOS))) + n_dr = len(DAMPING_RATIOS) + response = np.zeros(shape=(freqs.shape[0], n_dr)) + for i in range(n_dr): + response[:, i] = estimate_shaper( + np, shaper, DAMPING_RATIOS[i], freqs) + legend = ['damping ratio = %.3f' % d_r for d_r in DAMPING_RATIOS] return freqs, response, legend +def step_response(t, omega, damping_ratio): + t = np.maximum(t, 0.) + damping = damping_ratio * omega + omega_d = omega * math.sqrt(1. - damping_ratio**2) + phase = math.acos(damping_ratio) + return (1. - np.exp(np.outer(-damping, t)) + * np.sin(np.outer(omega_d, t) + phase) * (1. / math.sin(phase))) -def gen_shaped_step_function(shaper): +def gen_shaped_step_function(shaper, freq, damping_ratio): # Calculate shaping of a step function A, T, _ = shaper inv_D = 1.0 / sum(A) n = len(T) + t_s = T[-1] - T[0] + A = np.asarray(A) - omega = 2.0 * math.pi * STEP_SIMULATION_RESONANCE_FREQ - damping = STEP_SIMULATION_DAMPING_RATIO * omega - omega_d = omega * math.sqrt(1.0 - STEP_SIMULATION_DAMPING_RATIO**2) - phase = math.acos(STEP_SIMULATION_DAMPING_RATIO) + omega = 2. * math.pi * freq - t_start = T[0] - 0.5 / SHAPER_FREQ - t_end = T[-1] + 1.5 / STEP_SIMULATION_RESONANCE_FREQ - result = [] - time = [] - t = t_start + t_start = T[0] - .5 * t_s + t_end = T[-1] + 1.5 * max(1. / freq, t_s) + dt = .01 * min(t_s, 1. / freq) + time = np.arange(t_start, t_end, dt) + time_t = time[np.newaxis].T - def step_response(t): - if t < 0.0: - return 0.0 - return 1.0 - math.exp(-damping * t) * math.sin( - omega_d * t + phase - ) / math.sin(phase) + step = np.zeros(time.shape) + step[time >= 0.] = 1. + commanded = np.sum(A * np.where(time_t - T >= 0, 1., 0.), axis=-1) * inv_D + response = np.zeros(shape=(time.shape)) + for i in range(n): + response += A[i] * step_response(time - T[i], omega, damping_ratio)[0,:] + response *= inv_D + velocity = np.insert( + (response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.) + legend = ['step', 'shaper commanded', 'system response', + 'system response velocity'] + result = np.zeros(shape=(time.shape[0], 4)) + result[:,0] = step + result[:,1] = commanded + result[:,2] = response + result[:,3] = velocity + return time, result, legend - while t <= t_end: - val = [] - val.append(1.0 if t >= 0.0 else 0.0) - # val.append(step_response(t)) +def gen_smoothed_step_function(smoother, freq, damping_ratio): + # Calculate smoothing of a step function + C, t_sm, t_offs, _ = smoother + hst = 0.5 * t_sm - commanded = 0.0 - response = 0.0 - S = C = 0 - for i in range(n): - if t < T[i]: - continue - commanded += A[i] - response += A[i] * step_response(t - T[i]) - val.append(commanded * inv_D) - val.append(response * inv_D) + n_t = max(1000, 1000 * round(t_sm * freq)) + tau, dt = np.linspace(-hst, hst, n_t, retstep=True) + w = np.zeros(shape=tau.shape) + for c in C[::-1]: + w = w * tau + c - result.append(val) - time.append(t) - t += 0.01 / SHAPER_FREQ - legend = ["step", "shaper commanded", "system response"] + omega = 2. * math.pi * freq + damping = damping_ratio * omega + omega_d = omega * math.sqrt(1. - damping_ratio**2) + phase = math.acos(damping_ratio) + + t_start = -t_sm - t_offs + t_end = hst - t_offs + 1.5 * max(1. / freq, t_sm) + time = np.arange(t_start, t_end, dt) + time_t = time[np.newaxis].T + w_dt = w * dt + step = np.zeros(time.shape) + step[time >= 0.] = 1. + commanded = np.sum(w_dt * np.where(time_t - tau + t_offs >= 0, 1., 0.), + axis=-1) + s_r_n = time.size + tau.size - 1 + s_r_t = np.arange(t_start - hst, t_end + hst * 1.1, dt)[:s_r_n] + s_r = step_response(s_r_t + t_offs, omega, damping_ratio) + response = np.convolve(s_r[0,:], w_dt, mode='valid') + velocity = np.insert((response[1:] - response[:-1]) + / (2 * math.pi * freq * dt), 0, 0.) + legend = ['step', 'shaper commanded', 'system response', + 'system response velocity'] + result = np.zeros(shape=(time.shape[0], 4)) + result[:,0] = step + result[:,1] = commanded + result[:,2] = response + result[:,3] = velocity return time, result, legend - -def plot_shaper(shaper): - shift_pulses(shaper) - freqs, response, response_legend = gen_shaper_response(shaper) - time, step_vals, step_legend = gen_shaped_step_function(shaper) - - fig, (ax1, ax2) = matplotlib.pyplot.subplots(nrows=2, figsize=(10, 9)) - ax1.set_title( - "Vibration response simulation for shaper '%s',\n" - "shaper_freq=%.1f Hz, damping_ratio=%.3f" - % (shaper[-1], SHAPER_FREQ, SHAPER_DAMPING_RATIO) - ) +def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): + for s in shaper_defs.INPUT_SHAPERS: + if s.name == shaper_name.lower(): + A, T = s.init_func(shaper_freq, SHAPER_DAMPING_RATIO) + ts = shaper_calibrate.get_shaper_offset(A, T) + T = [t - ts for t in T] + shaper = A, T, s.name.upper() + freqs, response, response_legend = gen_shaper_response( + shaper, shaper_freq, shaper_calibrate.estimate_shaper) + time, step_vals, step_legend = gen_shaped_step_function( + shaper, freq, damping_ratio) + for s in shaper_defs.INPUT_SMOOTHERS: + if s.name == shaper_name.lower(): + C, t_sm = s.init_func(shaper_freq) + t_offs = shaper_calibrate.get_smoother_offset(np, C, t_sm) + shaper = C, t_sm, t_offs, s.name.upper() + freqs, response, response_legend = gen_shaper_response( + shaper, shaper_freq, shaper_calibrate.estimate_smoother) + time, step_vals, step_legend = gen_smoothed_step_function( + shaper, freq, damping_ratio) + + fig, (ax1, ax2) = matplotlib.pyplot.subplots(nrows=2, figsize=(10,9)) + ax1.set_title("Vibration response simulation for shaper '%s',\n" + "shaper_freq=%.1f Hz, damping_ratio=%.3f" + % (shaper[-1], shaper_freq, SHAPER_DAMPING_RATIO)) ax1.plot(freqs, response) ax1.set_ylim(bottom=0.0) fontP = matplotlib.font_manager.FontProperties() @@ -258,11 +192,8 @@ def plot_shaper(shaper): ax1.grid(which="major", color="grey") ax1.grid(which="minor", color="lightgrey") - ax2.set_title( - "Unit step input, resonance frequency=%.1f Hz, " - "damping ratio=%.3f" - % (STEP_SIMULATION_RESONANCE_FREQ, STEP_SIMULATION_DAMPING_RATIO) - ) + ax2.set_title("Unit step input, resonance frequency=%.1f Hz, " + "damping ratio=%.3f" % (freq, damping_ratio)) ax2.plot(time, step_vals) ax2.legend(step_legend, loc="best", prop=fontP) ax2.set_xlabel("Time, sec") @@ -284,21 +215,32 @@ def main(): # Parse command-line arguments usage = "%prog [options]" opts = optparse.OptionParser(usage) - opts.add_option( - "-o", - "--output", - type="string", - dest="output", - default=None, - help="filename of output graph", - ) + opts.add_option("-o", "--output", type="string", dest="output", + default=None, help="filename of output graph") + opts.add_option("-s", "--shaper", type="string", dest="shaper", + default="ei", help="name of the shaper to plot") + opts.add_option("-f", "--freq", type="float", dest="freq", + default=STEP_SIMULATION_RESONANCE_FREQ, + help="the frequency of the system") + opts.add_option("--damping_ratio", type="float", dest="damping_ratio", + default=STEP_SIMULATION_DAMPING_RATIO, + help="the damping ratio of the system") + opts.add_option("--shaper_freq", type="float", dest="shaper_freq", + default=SHAPER_FREQ, + help="the frequency of the shaper") options, args = opts.parse_args() if len(args) != 0: opts.error("Incorrect number of arguments") + if (options.shaper.lower() not in + [s.name for s in shaper_defs.INPUT_SHAPERS] and + options.shaper.lower() not in + [s.name for s in shaper_defs.INPUT_SMOOTHERS]): + opts.error("Invalid shaper name '%s'" % (opts.shaper,)) # Draw graph setup_matplotlib(options.output is not None) - fig = plot_shaper(get_shaper()) + fig = plot_shaper(options.shaper, options.shaper_freq, options.freq, + options.damping_ratio) # Show graph if options.output is None: From 1bb4236181820ba16c13cff8357db3938e85e876 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 24 May 2023 19:25:09 +0200 Subject: [PATCH 26/35] input_shaper: Added smooth_zvd_ei smoother Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_calibrate.py | 4 ++-- klippy/extras/shaper_defs.py | 8 ++++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index c9c625661..b8bcf1310 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -18,8 +18,8 @@ TEST_DAMPING_RATIOS = [0.075, 0.1, 0.15] -AUTOTUNE_SHAPERS = ['mzv', 'ei', '2hump_ei', 'smooth_zv', 'smooth_mzv', - 'smooth_ei', 'smooth_2hump_ei', 'smooth_si'] +AUTOTUNE_SHAPERS = ['smooth_zv', 'smooth_mzv', 'smooth_ei', 'smooth_2hump_ei', + 'smooth_zvd_ei', 'smooth_si', 'mzv', 'ei', '2hump_ei'] ###################################################################### # Frequency response calculation and shaper auto-tuning diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index 0e27266b2..d259f5fc5 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -173,6 +173,13 @@ def get_si_smoother(shaper_freq, damping_ratio_unused=None, 4.234582468800491,-2.226157642004671,1.267781046297883] return init_smoother(coeffs, 1.245 / shaper_freq, normalize_coeffs) +def get_zvd_ei_smoother(shaper_freq, damping_ratio_unused=None, + normalize_coeffs=True): + coeffs = [-18835.07746719777,1914.349309746547,8786.608981369287, + -807.3061869131075,-1209.429748155012,96.48879052981883, + 43.1595785340444,-3.577268915175282,1.083220648523371] + return init_smoother(coeffs, 1.475 / shaper_freq, normalize_coeffs) + # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ InputShaperCfg("zv", get_zv_shaper, min_freq=21.0), @@ -188,5 +195,6 @@ def get_si_smoother(shaper_freq, damping_ratio_unused=None, InputSmootherCfg('smooth_mzv', get_mzv_smoother, min_freq=23.), InputSmootherCfg('smooth_ei', get_ei_smoother, min_freq=23.), InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=23.), + InputSmootherCfg('smooth_zvd_ei', get_zvd_ei_smoother, min_freq=23.), InputSmootherCfg('smooth_si', get_si_smoother, min_freq=23.), ] From 375633fefeff27e9b6f34b2fd67ef71b0f0a869a Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 24 May 2023 19:33:08 +0200 Subject: [PATCH 27/35] shaper_calibrate: Use system backwards velocity for shaper estimations Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_calibrate.py | 93 ++++++++++++++++++++++++++++++- scripts/graph_shaper.py | 14 ++--- 2 files changed, 95 insertions(+), 12 deletions(-) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index b8bcf1310..49167efad 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -90,7 +90,25 @@ def get_smoother_offset(np, C, t_sm): return -np.trapz(t * w, dx=dt) -def estimate_shaper(np, shaper, test_damping_ratio, test_freqs): +def step_response(np, t, omega, damping_ratio): + t = np.maximum(t, 0.) + omega = np.swapaxes(np.array(omega, ndmin=2), 0, 1) + damping = damping_ratio * omega + omega_d = omega * math.sqrt(1. - damping_ratio**2) + phase = math.acos(damping_ratio) + return (1. - np.exp((-damping * t)) + * np.sin((omega_d * t) + phase) * (1. / math.sin(phase))) + +def step_response_min_velocity(damping_ratio): + d2 = damping_ratio * damping_ratio + d_r = damping_ratio / math.sqrt(1. - d2) + # Analytical formula for the minimum was obtained using Maxima system + t = .5 * math.atan2(2. * d2, (2. * d2 - 1.) * d_r) + math.pi + phase = math.acos(damping_ratio) + v = math.exp(-d_r * t) * (d_r * math.sin(t + phase) - math.cos(t + phase)) + return v + +def estimate_shaper_old(np, shaper, test_damping_ratio, test_freqs): A, T = np.asarray(shaper[0]), np.asarray(shaper[1]) inv_D = 1. / A.sum() @@ -102,7 +120,36 @@ def estimate_shaper(np, shaper, test_damping_ratio, test_freqs): C = W * np.cos(np.outer(omega_d, T)) return np.sqrt(S.sum(axis=1)**2 + C.sum(axis=1)**2) * inv_D -def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): +def estimate_shaper(np, shaper, test_damping_ratio, test_freqs): + A, T = np.asarray(shaper[0]), np.asarray(shaper[1]) + inv_D = 1. / A.sum() + n = len(T) + t_s = T[-1] - T[0] + + test_freqs = np.asarray(test_freqs) + t_start = T[0] + t_end = T[-1] + 2.0 * np.maximum(1. / test_freqs[test_freqs > 0.], t_s) + n_t = 1000 + unity_range = np.linspace(0., 1., n_t) + time = (t_end[:, np.newaxis] - t_start) * unity_range + t_start + dt = (time[:,-1] - time[:,0]) / n_t + + min_v = -step_response_min_velocity(test_damping_ratio) + + omega = 2. * math.pi * test_freqs[test_freqs > 0.] + + response = np.zeros(shape=(omega.shape[0], time.shape[-1])) + for i in range(n): + s_r = step_response(np, time - T[i], omega, test_damping_ratio) + response += A[i] * s_r + response *= inv_D + velocity = (response[:,1:] - response[:,:-1]) / (omega * dt)[:, np.newaxis] + res = np.zeros(shape=test_freqs.shape) + res[test_freqs > 0.] = -velocity.min(axis=-1) / min_v + res[test_freqs <= 0.] = 1. + return res + +def estimate_smoother_old(np, smoother, test_damping_ratio, test_freqs): C, t_sm = smoother[0], smoother[1] hst = t_sm * 0.5 @@ -122,6 +169,48 @@ def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): S = np.sin(np.outer(omega_d, (t-t_sm))) return np.sqrt(np.trapz(E * C, dx=dt)**2 + np.trapz(E * S, dx=dt)**2) +def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): + C, t_sm = smoother[0], smoother[1] + hst = t_sm * 0.5 + + test_freqs = np.asarray(test_freqs) + + t_start = -hst + t_end = hst + 1.5 * np.maximum(1. / test_freqs[test_freqs > 0.], t_sm) + n_t = 1000 + unity_range = np.linspace(0., 1., n_t) + time = (t_end[:, np.newaxis] - t_start) * unity_range + t_start + dt = (time[:,-1] - time[:,0]) / n_t + tau = np.copy(time) + tau[time > hst] = 0. + + w = np.zeros(shape=tau.shape) + for c in C[::-1]: + w = w * tau + c + w[time > hst] = 0. + norms = (w * dt[:, np.newaxis]).sum(axis=-1) + + min_v = -step_response_min_velocity(test_damping_ratio) + + omega = 2. * math.pi * test_freqs[test_freqs > 0.] + + wl = np.count_nonzero(time <= hst, axis=-1).max() + + def get_windows(m, wl): + nrows = m.shape[-1] - wl + 1 + n = m.strides[-1] + return np.lib.stride_tricks.as_strided(m, shape=(m.shape[0], nrows, wl), + strides=(m.strides[0], n, n)) + + s_r = step_response(np, time, omega, test_damping_ratio) + w_dt = w[:, :wl] * (np.reciprocal(norms) * dt)[:, np.newaxis] + response = np.einsum("ijk,ik->ij", get_windows(s_r, wl), w_dt[:,::-1]) + velocity = (response[:,1:] - response[:,:-1]) / (omega * dt)[:, np.newaxis] + res = np.zeros(shape=test_freqs.shape) + res[test_freqs > 0.] = -velocity.min(axis=-1) / min_v + res[test_freqs <= 0.] = 1. + return res + class ShaperCalibrate: def __init__(self, printer): self.printer = printer diff --git a/scripts/graph_shaper.py b/scripts/graph_shaper.py index 7536504db..bea3feaee 100755 --- a/scripts/graph_shaper.py +++ b/scripts/graph_shaper.py @@ -73,14 +73,6 @@ def gen_shaper_response(shaper, shaper_freq, estimate_shaper): legend = ['damping ratio = %.3f' % d_r for d_r in DAMPING_RATIOS] return freqs, response, legend -def step_response(t, omega, damping_ratio): - t = np.maximum(t, 0.) - damping = damping_ratio * omega - omega_d = omega * math.sqrt(1. - damping_ratio**2) - phase = math.acos(damping_ratio) - return (1. - np.exp(np.outer(-damping, t)) - * np.sin(np.outer(omega_d, t) + phase) * (1. / math.sin(phase))) - def gen_shaped_step_function(shaper, freq, damping_ratio): # Calculate shaping of a step function A, T, _ = shaper @@ -102,7 +94,8 @@ def gen_shaped_step_function(shaper, freq, damping_ratio): commanded = np.sum(A * np.where(time_t - T >= 0, 1., 0.), axis=-1) * inv_D response = np.zeros(shape=(time.shape)) for i in range(n): - response += A[i] * step_response(time - T[i], omega, damping_ratio)[0,:] + response += A[i] * shaper_calibrate.step_response( + np, time - T[i], omega, damping_ratio)[0,:] response *= inv_D velocity = np.insert( (response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.) @@ -142,7 +135,8 @@ def gen_smoothed_step_function(smoother, freq, damping_ratio): axis=-1) s_r_n = time.size + tau.size - 1 s_r_t = np.arange(t_start - hst, t_end + hst * 1.1, dt)[:s_r_n] - s_r = step_response(s_r_t + t_offs, omega, damping_ratio) + s_r = shaper_calibrate.step_response( + np, s_r_t + t_offs, omega, damping_ratio) response = np.convolve(s_r[0,:], w_dt, mode='valid') velocity = np.insert((response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.) From 8ae68cfc4cf1132c9b0961e100f4b8eb5210586e Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 24 May 2023 22:16:56 +0200 Subject: [PATCH 28/35] shaper_calibrate: A modified shaper calibration approach Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_calibrate.py | 131 ++++++++++++++---------------- scripts/calibrate_shaper.py | 27 +++--- 2 files changed, 77 insertions(+), 81 deletions(-) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index 49167efad..fdf96eb07 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -34,38 +34,25 @@ def __init__(self, freq_bins, psd_sum, psd_x, psd_y, psd_z): self.psd_y = psd_y self.psd_z = psd_z self._psd_list = [self.psd_sum, self.psd_x, self.psd_y, self.psd_z] - self._psd_map = { - "x": self.psd_x, - "y": self.psd_y, - "z": self.psd_z, - "all": self.psd_sum, - } - self.data_sets = 1 - + self._psd_map = {'x': self.psd_x, 'y': self.psd_y, 'z': self.psd_z, + 'all': self.psd_sum} def add_data(self, other): np = self.numpy - joined_data_sets = self.data_sets + other.data_sets for psd, other_psd in zip(self._psd_list, other._psd_list): # `other` data may be defined at different frequency bins, # interpolating to fix that. - other_normalized = other.data_sets * np.interp( - self.freq_bins, other.freq_bins, other_psd - ) - psd *= self.data_sets - psd[:] = (psd + other_normalized) * (1.0 / joined_data_sets) - self.data_sets = joined_data_sets - + other_normalized = np.interp( + self.freq_bins, other.freq_bins, other_psd) + psd[:] = np.maximum(psd, other_normalized) def set_numpy(self, numpy): self.numpy = numpy def normalize_to_frequencies(self): + freq_bins = self.freq_bins for psd in self._psd_list: - # Avoid division by zero errors - psd /= self.freq_bins + 0.1 - # Remove low-frequency noise - psd[self.freq_bins < MIN_FREQ] = 0.0 - - def get_psd(self, axis="all"): + # Avoid division by zero errors and remove low-frequency noise + psd *= self.numpy.tanh(.5 / MIN_FREQ * freq_bins) / (freq_bins + .1) + def get_psd(self, axis='all'): return self._psd_map[axis] @@ -81,7 +68,7 @@ def get_shaper_offset(A, T): def get_smoother_offset(np, C, t_sm): hst = t_sm * 0.5 - n_t = 1000 + n_t = 200 t, dt = np.linspace(-hst, hst, n_t, retstep=True) w = np.zeros(shape=t.shape) @@ -342,15 +329,15 @@ def process_accelerometer_data(self, data): calibration_data.set_numpy(self.numpy) return calibration_data - def _estimate_remaining_vibrations(self, vals, psd): - # The input shaper can only reduce the amplitude of vibrations by - # SHAPER_VIBRATION_REDUCTION times, so all vibrations below that - # threshold can be igonred - vibr_threshold = psd.max() / shaper_defs.SHAPER_VIBRATION_REDUCTION - remaining_vibrations = self.numpy.maximum( - vals * psd - vibr_threshold, 0 - ).sum() - all_vibrations = self.numpy.maximum(psd - vibr_threshold, 0).sum() + def _estimate_remaining_vibrations(self, freq_bins, vals, psd): + # Calculate the acceptable level of remaining vibrations. + # Note that these are not true remaining vibrations, but rather + # just a score to compare different shapers between each other. + vibr_threshold = ((psd[freq_bins > 0] / freq_bins[freq_bins > 0]).max() + * (freq_bins + MIN_FREQ) * (1. / 33.3)) + remaining_vibrations = (self.numpy.maximum( + vals * psd - vibr_threshold, 0) * freq_bins**2).sum() + all_vibrations = (psd * freq_bins**2).sum() return remaining_vibrations / all_vibrations def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): @@ -384,7 +371,9 @@ def _get_smoother_smoothing(self, smoother, accel=5000, scv=5.): w = np.zeros(shape=t.shape) for c in C[::-1]: w = w * (-t) + c - t += get_smoother_offset(np, C, t_sm) + inv_norm = 1. / np.trapz(w, dx=dt) + w *= inv_norm + t -= np.trapz(t * w, dx=dt) offset_180 = np.trapz(half_accel * t**2 * w, dx=dt) offset_90_x = np.trapz(((scv + half_accel * t) * t * w)[t >= 0], dx=dt) @@ -396,7 +385,17 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, estimate_shaper, get_shaper_smoothing): np = self.numpy - test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, 0.2) + shaper = shaper_cfg.init_func(1.0, shaper_defs.DEFAULT_DAMPING_RATIO) + + test_freq_bins = np.arange(0., 10., 0.01) + test_shaper_vals = np.zeros(shape=test_freq_bins.shape) + # Exact damping ratio of the printer is unknown, pessimizing + # remaining vibrations over possible damping values + for dr in TEST_DAMPING_RATIOS: + vals = estimate_shaper(self.numpy, shaper, dr, test_freq_bins) + test_shaper_vals = np.maximum(test_shaper_vals, vals) + + test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, .2) freq_bins = calibration_data.freq_bins psd = calibration_data.psd_sum[freq_bins <= MAX_FREQ] @@ -405,28 +404,22 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, best_res = None results = [] for test_freq in test_freqs[::-1]: - shaper_vibrations = 0.0 - shaper_vals = np.zeros(shape=freq_bins.shape) shaper = shaper_cfg.init_func( test_freq, shaper_defs.DEFAULT_DAMPING_RATIO) shaper_smoothing = get_shaper_smoothing(shaper) if max_smoothing and shaper_smoothing > max_smoothing and best_res: return best_res - # Exact damping ratio of the printer is unknown, pessimizing - # remaining vibrations over possible damping values - for dr in TEST_DAMPING_RATIOS: - vals = estimate_shaper(self.numpy, shaper, dr, freq_bins) - vibrations = self._estimate_remaining_vibrations(vals, psd) - shaper_vals = np.maximum(shaper_vals, vals) - if vibrations > shaper_vibrations: - shaper_vibrations = vibrations + shaper_vals = np.interp(freq_bins, test_freq_bins * test_freq, + test_shaper_vals) + shaper_vibrations = self._estimate_remaining_vibrations( + freq_bins, shaper_vals, psd) max_accel = self.find_max_accel(shaper, get_shaper_smoothing) # The score trying to minimize vibrations, but also accounting # the growth of smoothing. The formula itself does not have any # special meaning, it simply shows good results on real user data - shaper_score = shaper_smoothing * ( - shaper_vibrations**1.5 + shaper_vibrations * 0.2 + 0.01 - ) + shaper_score = shaper_smoothing * (2. * shaper_vibrations**1.5 + + shaper_vibrations * .2 + .001 + + shaper_smoothing * .002) results.append( CalibrationResult( name=shaper_cfg.name, @@ -445,20 +438,22 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, # much worse than the 'best' one, but gives much less smoothing selected = best_res for res in results[::-1]: - if res.vibrs < best_res.vibrs * 1.01 and res.score < selected.score: + if res.score < selected.score and ( + res.vibrs < best_res.vibrs * 1.2 or + res.vibrs < best_res.vibrs + 0.0075): selected = res return selected - def _bisect(self, func): - left = right = 1.0 + def _bisect(self, func, eps = 1e-8): + left = right = 1. while not func(left): right = left left *= 0.5 if right == left: while func(right): - right *= 2.0 - while right - left > 1e-8: - middle = (left + right) * 0.5 + right *= 2. + while right - left > eps: + middle = (left + right) * .5 if func(middle): left = middle else: @@ -470,7 +465,7 @@ def find_max_accel(self, s, get_smoothing): # for max_accel without much smoothing TARGET_SMOOTHING = 0.12 max_accel = self._bisect(lambda test_accel: get_smoothing( - s, test_accel) <= TARGET_SMOOTHING) + s, test_accel) <= TARGET_SMOOTHING, 1e-2) return max_accel def find_best_shaper(self, calibration_data, max_smoothing, logger=None): @@ -484,9 +479,10 @@ def find_best_shaper(self, calibration_data, max_smoothing, logger=None): estimate_smoother, self._get_smoother_smoothing)) if logger is not None: logger("Fitted smoother '%s' frequency = %.1f Hz " - "(vibrations = %.1f%%, smoothing ~= %.3f)" % ( + "(vibration score = %.2f%%, smoothing ~= %.3f," + " combined score = %.3e)" % ( smoother.name, smoother.freq, smoother.vibrs * 100., - smoother.smoothing)) + smoother.smoothing, smoother.score)) logger("To avoid too much smoothing with '%s', suggested " "max_accel <= %.0f mm/sec^2" % ( smoother.name, @@ -505,21 +501,14 @@ def find_best_shaper(self, calibration_data, max_smoothing, logger=None): shaper_cfg, calibration_data, max_smoothing, estimate_shaper, self._get_shaper_smoothing)) if logger is not None: - logger( - "Fitted shaper '%s' frequency = %.1f Hz " - "(vibrations = %.1f%%, smoothing ~= %.3f)" - % ( - shaper.name, - shaper.freq, - shaper.vibrs * 100.0, - shaper.smoothing, - ) - ) - logger( - "To avoid too much smoothing with '%s', suggested " - "max_accel <= %.0f mm/sec^2" - % (shaper.name, round(shaper.max_accel / 100.0) * 100.0) - ) + logger("Fitted shaper '%s' frequency = %.1f Hz " + "(vibration score = %.2f%%, smoothing ~= %.3f," + " combined score = %.3e)" % ( + shaper.name, shaper.freq, shaper.vibrs * 100., + shaper.smoothing, shaper.score)) + logger("To avoid too much smoothing with '%s', suggested " + "max_accel <= %.0f mm/sec^2" % ( + shaper.name, round(shaper.max_accel / 100.) * 100.)) all_shapers.append(shaper) if (best_shaper is None or shaper.score * 1.2 < best_shaper.score or (shaper.score * 1.03 < best_shaper.score and diff --git a/scripts/calibrate_shaper.py b/scripts/calibrate_shaper.py index 3e2b2cc49..b386c5333 100755 --- a/scripts/calibrate_shaper.py +++ b/scripts/calibrate_shaper.py @@ -110,18 +110,22 @@ def plot_freq_response( best_shaper_vals = None for shaper in shapers: label = "%s (%.1f Hz, vibr=%.1f%%, sm~=%.2f, accel<=%.f)" % ( - shaper.name.upper(), - shaper.freq, - shaper.vibrs * 100.0, - shaper.smoothing, - round(shaper.max_accel / 100.0) * 100.0, - ) - linestyle = "dotted" + shaper.name.upper(), shaper.freq, + shaper.vibrs * 100., shaper.smoothing, + round(shaper.max_accel / 100.) * 100.) + linestyle = 'dotted' if shaper.name.startswith('smooth') else 'dashed' + linewidth = 1.0 if shaper.name == selected_shaper: - linestyle = "dashdot" + linestyle = 'dashdot' + linewidth = 2.0 best_shaper_vals = shaper.vals - ax2.plot(freqs, shaper.vals, label=label, linestyle=linestyle) - ax.plot(freqs, psd * best_shaper_vals, label="After\nshaper", color="cyan") + ax2.plot(freqs, shaper.vals, label=label, linestyle=linestyle, + linewidth=linewidth) + vibr_thresh = (psd[freqs > 0] / freqs[freqs > 0]).max() * (freqs + 5) / 33.3 + ax.plot(freqs, vibr_thresh, + label='Acceptable\nvibrations', color='lightgrey') + ax.plot(freqs, psd * best_shaper_vals, + label='After\nshaper', color='cyan') # A hack to add a human-readable shaper recommendation to legend ax2.plot( [], [], " ", label="Recommended shaper: %s" % (selected_shaper.upper()) @@ -130,6 +134,9 @@ def plot_freq_response( ax.legend(loc="upper left", prop=fontP) ax2.legend(loc="upper right", prop=fontP) + ax.set_ylim(bottom=0) + ax2.set_ylim(bottom=0) + fig.tight_layout() return fig From 8d918bb885ad3e933b5b432f4a48d53f3acc0ff0 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 24 May 2023 22:18:18 +0200 Subject: [PATCH 29/35] input_shaper: Updated minimum smoother frequencies Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_defs.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index d259f5fc5..935e3fd5b 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -190,11 +190,12 @@ def get_zvd_ei_smoother(shaper_freq, damping_ratio_unused=None, InputShaperCfg("3hump_ei", get_3hump_ei_shaper, min_freq=48.0), ] +# min_freq for each smoother is chosen to have projected max_accel ~= 1000 INPUT_SMOOTHERS = [ - InputSmootherCfg('smooth_zv', get_zv_smoother, min_freq=21.), - InputSmootherCfg('smooth_mzv', get_mzv_smoother, min_freq=23.), - InputSmootherCfg('smooth_ei', get_ei_smoother, min_freq=23.), - InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=23.), - InputSmootherCfg('smooth_zvd_ei', get_zvd_ei_smoother, min_freq=23.), - InputSmootherCfg('smooth_si', get_si_smoother, min_freq=23.), + InputSmootherCfg('smooth_zv', get_zv_smoother, min_freq=18.), + InputSmootherCfg('smooth_mzv', get_mzv_smoother, min_freq=20.), + InputSmootherCfg('smooth_ei', get_ei_smoother, min_freq=21.), + InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=21.5), + InputSmootherCfg('smooth_zvd_ei', get_zvd_ei_smoother, min_freq=26.), + InputSmootherCfg('smooth_si', get_si_smoother, min_freq=21.5), ] From 1092d0d6d4c9bdd846bab1e9a876dbb144857e28 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Sun, 11 Jun 2023 23:41:55 +0200 Subject: [PATCH 30/35] input_shaper: Added customized smoothers for extruder This makes use of different smoothers than input smoothers themselves when extruder syncrhonization with input shaping is enabled. The old behavior is available with the following command: `ENABLE_INPUT_SHAPER EXTRUDER=... EXACT=1` Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 2 +- klippy/chelper/kin_extruder.c | 3 +- klippy/extras/input_shaper.py | 71 ++++++++++++++++++++++------- klippy/extras/shaper_defs.py | 85 +++++++++++++++++++++++++++++++++++ klippy/kinematics/extruder.py | 11 ++--- 5 files changed, 149 insertions(+), 23 deletions(-) diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 333fa8f40..243bbcd9f 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -155,7 +155,7 @@ int extruder_set_shaper_params(struct stepper_kinematics *sk, char axis , int n, double a[], double t[]); int extruder_set_smoothing_params(struct stepper_kinematics *sk, char axis - , int n, double a[], double t_sm); + , int n, double a[], double t_sm, double t_offs); double extruder_get_step_gen_window(struct stepper_kinematics *sk); """ diff --git a/klippy/chelper/kin_extruder.c b/klippy/chelper/kin_extruder.c index 06881c5cf..50f33f967 100644 --- a/klippy/chelper/kin_extruder.c +++ b/klippy/chelper/kin_extruder.c @@ -294,13 +294,14 @@ extruder_set_shaper_params(struct stepper_kinematics *sk, char axis int __visible extruder_set_smoothing_params(struct stepper_kinematics *sk, char axis - , int n, double a[], double t_sm) + , int n, double a[], double t_sm, double t_offs) { if (axis != 'x' && axis != 'y' && axis != 'z') return -1; struct extruder_stepper *es = container_of(sk, struct extruder_stepper, sk); struct smoother *sm = &es->sm[axis-'x']; int status = init_smoother(n, a, t_sm, sm); + sm->t_offs = t_offs; extruder_note_generation_time(es); return status; } diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 7fcb948a7..7831e1561 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -1,7 +1,7 @@ # Kinematic input shaper to minimize motion vibrations in XY plane # # Copyright (C) 2019-2020 Kevin O'Connor -# Copyright (C) 2020 Dmitry Butyugin +# Copyright (C) 2020-2023 Dmitry Butyugin # # This file may be distributed under the terms of the GNU GPLv3 license. import collections @@ -133,6 +133,7 @@ class AxisInputShaper: def __init__(self, params): self.params = params self.n, self.A, self.T = params.get_shaper() + self.t_offs = self._calc_offset() self.saved = None def get_name(self): return 'shaper_' + self.get_axis() @@ -140,13 +141,18 @@ def get_type(self): return self.params.get_type() def get_axis(self): return self.params.get_axis() - def is_smoothing(self): - return False + def is_extruder_smoothing(self, exact_mode): + return not exact_mode and self.A def is_enabled(self): return self.n > 0 + def _calc_offset(self): + if not self.A: + return 0. + return sum([a * t for a, t in zip(self.A, self.T)]) / sum(self.A) def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.A, self.T = self.params.get_shaper() + self.t_offs = self._calc_offset() def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() @@ -157,15 +163,25 @@ def update_stepper_kinematics(self, sk): ffi_lib.input_shaper_set_shaper_params( sk, axis, self.n, self.A, self.T) return success - def update_extruder_kinematics(self, sk): + def update_extruder_kinematics(self, sk, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() - # Make sure to disable any active input smoothing - coeffs, smooth_time = [], 0. - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, len(coeffs), coeffs, smooth_time) == 0 - success = ffi_lib.extruder_set_shaper_params( - sk, axis, self.n, self.A, self.T) == 0 + if not self.is_extruder_smoothing(exact_mode): + # Make sure to disable any active input smoothing + coeffs, smooth_time = [], 0. + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, len(coeffs), coeffs, smooth_time, 0.) == 0 + success = ffi_lib.extruder_set_shaper_params( + sk, axis, self.n, self.A, self.T) == 0 + else: + shaper_type = self.get_type() + extr_smoother_func = shaper_defs.EXTURDER_SMOOTHERS.get( + shaper_type, shaper_defs.EXTURDER_SMOOTHERS['default']) + C_e, t_sm = extr_smoother_func(self.T[-1] - self.T[0], + normalize_coeffs=False) + smoother_offset = self.t_offs - 0.5 * (self.T[0] + self.T[-1]) + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, len(C_e), C_e, t_sm, smoother_offset) == 0 if not success: self.disable_shaping() ffi_lib.extruder_set_shaper_params( @@ -267,6 +283,7 @@ class AxisInputSmoother: def __init__(self, params): self.params = params self.n, self.coeffs, self.smooth_time = params.get_smoother() + self.t_offs = self._calc_offset() self.saved_smooth_time = 0. def get_name(self): return 'smoother_' + self.axis @@ -274,13 +291,22 @@ def get_type(self): return self.params.get_type() def get_axis(self): return self.params.get_axis() - def is_smoothing(self): + def is_extruder_smoothing(self, exact_mode): return True def is_enabled(self): return self.smooth_time > 0. + def _calc_offset(self): + int_t0 = int_t1 = 0. + for i, c in enumerate(self.coeffs): + if i & 1: + int_t1 += c / (2**(i+1) * (i+2)) + else: + int_t0 += c / (2**i * (i+1)) + return int_t1 * self.smooth_time / int_t0 def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.coeffs, self.smooth_time = self.params.get_smoother() + self.t_offs = self._calc_offset() def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() @@ -291,14 +317,24 @@ def update_stepper_kinematics(self, sk): ffi_lib.input_shaper_set_smoother_params( sk, axis, self.n, self.coeffs, self.smooth_time) return success - def update_extruder_kinematics(self, sk): + def update_extruder_kinematics(self, sk, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() # Make sure to disable any active input shaping A, T = shaper_defs.get_none_shaper() ffi_lib.extruder_set_shaper_params(sk, axis, len(A), A, T) - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, self.n, self.coeffs, self.smooth_time) == 0 + if exact_mode: + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, self.n, self.coeffs, self.smooth_time, + self.t_offs) == 0 + else: + smoother_type = self.get_type() + extr_smoother_func = shaper_defs.EXTURDER_SMOOTHERS.get( + smoother_type, shaper_defs.EXTURDER_SMOOTHERS['default']) + C_e, t_sm = extr_smoother_func(self.smooth_time, + normalize_coeffs=False) + success = ffi_lib.extruder_set_smoothing_params( + sk, axis, len(C_e), C_e, t_sm, self.t_offs) == 0 if not success: self.disable_shaping() ffi_lib.extruder_set_smoothing_params( @@ -368,6 +404,7 @@ def __init__(self, config): self.printer.register_event_handler("klippy:connect", self.connect) self.toolhead = None self.extruders = [] + self.exact_mode = 0 self.config_extruder_names = config.getlist('enabled_extruders', []) self.shaper_factory = ShaperFactory() self.shapers = [self.shaper_factory.create_shaper('x', config), @@ -441,7 +478,8 @@ def _update_input_shaping(self, error=None): old_delay) for e in self.extruders: for es in e.get_extruder_steppers(): - failed_shapers.extend(es.update_input_shaping(self.shapers)) + failed_shapers.extend(es.update_input_shaping(self.shapers, + self.exact_mode)) if failed_shapers: error = error or self.printer.command_error raise error( @@ -487,6 +525,7 @@ def cmd_ENABLE_INPUT_SHAPER(self, gcmd): msg += ("Cannot enable input shaper for AXIS='%s': " "was not disabled\n" % (axis_str,)) extruders = gcmd.get('EXTRUDER', '') + self.exact_mode = gcmd.get_int('EXACT', self.exact_mode) for en in extruders.split(','): extruder_name = en.strip() if not extruder_name: @@ -528,7 +567,7 @@ def cmd_DISABLE_INPUT_SHAPER(self, gcmd): if extruder in self.extruders: to_re_enable = [s for s in self.shapers if s.disable_shaping()] for es in extruder.get_extruder_steppers(): - es.update_input_shaping(self.shapers) + es.update_input_shaping(self.shapers, self.exact_mode) for shaper in to_re_enable: shaper.enable_shaping() self.extruders.remove(extruder) diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index 935e3fd5b..419ca6c63 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -180,6 +180,77 @@ def get_zvd_ei_smoother(shaper_freq, damping_ratio_unused=None, 43.1595785340444,-3.577268915175282,1.083220648523371] return init_smoother(coeffs, 1.475 / shaper_freq, normalize_coeffs) +def get_extr_zv_smoother(smooth_time, normalize_coeffs=True): + coeffs = [30., 0., -15., 0., 1.875] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_mzv_shaper_smoother(smooth_time, normalize_coeffs=True): + coeffs = [341.76438367888272,-27.466545717280418,-153.088062685115716, + 13.7332728586402091,12.4632094027673599,-1.7166591073300261, + 1.1121330721453511] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_zvd_shaper_smoother(smooth_time, normalize_coeffs=True): + coeffs = [630.74844408176386,-22.308531752627160,-307.90095218665919, + 11.1542658763135805,35.685142827998881,-1.3942832345391976, + 0.4670793658889200] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_ei_shaper_smoother(smooth_time, normalize_coeffs=True): + coeffs = [479.19339444799374,-30.811311356643330,-226.71074702571096, + 15.4056556783216649,23.506612053856646,-1.9257069597902081, + 0.8053718873928709] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_2hump_ei_shaper_smoother(smooth_time, normalize_coeffs=True): + coeffs = [6732.1368755766744,-451.56419782599357,-3806.9806651540493, + 208.13010436901791,666.92541201595748,-19.3967650921351584, + -40.412386015136598,-1.1032496589986802,1.6069214755974468] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_3hump_ei_shaper_smoother(smooth_time, normalize_coeffs=True): + coeffs = [4862.4802465588073,-540.91386960353532,-2666.8882179975822, + 256.61643727342101,445.67339875133428,-26.886868086047631, + -26.700173911045901,-0.8650310955216656,1.4965209988949661] + # Ideally, the following coefficients should be used, but this order + # is currently not supported by C integration routines + # coeffs = [101624.001181926069,-10140.6488068767758,-67034.280052740767, + # 5924.1010170704049,15212.5854866606205,-1110.64163123568596, + # -1371.73807717349200,78.342425346940715,38.307990049684413, + # -3.1226733743674644,1.0704518836439141] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_mzv_smoother(smooth_time, normalize_coeffs=True): + coeffs = [128.462077073626915,-42.165459650911941,-38.818969860871568, + 21.0827298254559707,-4.6771545208692649,-2.6353412281819963, + 1.5882542922463685] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_ei_smoother(smooth_time, normalize_coeffs=True): + coeffs = [276.191048111389079,-41.835391901208922,-117.95949005967274, + 20.9176959506044611,7.1939235089509097,-2.6147119938255576, + 1.2585021247513637] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_2hump_ei_smoother(smooth_time, normalize_coeffs=True): + coeffs = [1672.36152804372318,-70.342256841452126,-786.13315506927438, + -0.9529934764915708,102.73410996847843,13.665669896018062, + -8.4896839114829827,-2.2577576185761026,1.4522074338774609] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_zvd_ei_smoother(smooth_time, normalize_coeffs=True): + coeffs = [1829.54723817218109,-746.70578009311191,-927.21920387422937, + 393.61004675467614,145.568898884847072,-56.797689609879597, + -13.2775511017668428,1.2660722942575129,1.5624627565635203] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + +def get_extr_si_smoother(smooth_time, normalize_coeffs=True): + coeffs = [114.962413010742423,164.830749583661599,90.845373435141965, + -132.210144372387873,-42.617667050825957,35.199306639257386, + -2.9098248068475399,-3.1121730987848171,1.5225295066412017] + return init_smoother(coeffs, smooth_time, normalize_coeffs) + + # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ InputShaperCfg("zv", get_zv_shaper, min_freq=21.0), @@ -199,3 +270,17 @@ def get_zvd_ei_smoother(shaper_freq, damping_ratio_unused=None, InputSmootherCfg('smooth_zvd_ei', get_zvd_ei_smoother, min_freq=26.), InputSmootherCfg('smooth_si', get_si_smoother, min_freq=21.5), ] + +EXTURDER_SMOOTHERS = { + 'default' : get_extr_zv_smoother, + 'mzv' : get_extr_mzv_shaper_smoother, + 'zvd' : get_extr_zvd_shaper_smoother, + 'ei' : get_extr_ei_shaper_smoother, + '2hump_ei' : get_extr_2hump_ei_shaper_smoother, + '3hump_ei' : get_extr_3hump_ei_shaper_smoother, + 'smooth_mzv' : get_extr_mzv_smoother, + 'smooth_ei' : get_extr_ei_smoother, + 'smooth_2hump_ei' : get_extr_2hump_ei_smoother, + 'smooth_zvd_ei' : get_extr_zvd_ei_smoother, + 'smooth_si' : get_extr_si_smoother, +} diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index 52de6beca..ef6bd3be4 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -33,7 +33,7 @@ def update_extruder_kinematics(self, extruder_sk): for axis in self.axes: if not ffi_lib.extruder_set_smoothing_params( extruder_sk, axis.encode(), n, self.a, - smooth_time) == 0: + smooth_time, 0.) == 0: success = False return success def get_status(self, eventtime): @@ -253,18 +253,19 @@ def _update_pressure_advance(self, pa_model, time_offset): toolhead.note_step_generation_scan_time(new_delay, old_delay) self.pa_model = pa_model self.pressure_advance_time_offset = time_offset - def update_input_shaping(self, shapers): + def update_input_shaping(self, shapers, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) failed_shapers = [] for shaper in shapers: - if not shaper.update_extruder_kinematics(self.sk_extruder): + if not shaper.update_extruder_kinematics(self.sk_extruder, + exact_mode): failed_shapers.append(shaper) # Pressure advance requires extruder smoothing, make sure that # some smoothing is enabled - if shaper.is_smoothing() and shaper.is_enabled(): + if shaper.is_extruder_smoothing(exact_mode) and shaper.is_enabled(): self.smoother.disable_axis(shaper.get_axis()) - elif not shaper.is_smoothing() or not shaper.is_enabled(): + else: self.smoother.enable_axis(shaper.get_axis()) self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) From 7b74e5d0168342de6c7f4ec18c026f741e550730 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 21 Jun 2023 03:01:01 +0200 Subject: [PATCH 31/35] input_shaper: Moved shaper/smoother offset calculation functions Signed-off-by: Dmitry Butyugin --- klippy/extras/input_shaper.py | 22 ++++++---------------- klippy/extras/shaper_calibrate.py | 17 +---------------- klippy/extras/shaper_defs.py | 21 +++++++++++++++++++++ scripts/graph_shaper.py | 4 ++-- 4 files changed, 30 insertions(+), 34 deletions(-) diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 7831e1561..62e344e1c 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -133,7 +133,7 @@ class AxisInputShaper: def __init__(self, params): self.params = params self.n, self.A, self.T = params.get_shaper() - self.t_offs = self._calc_offset() + self.t_offs = shaper_defs.get_shaper_offset(self.A, self.T) self.saved = None def get_name(self): return 'shaper_' + self.get_axis() @@ -145,14 +145,10 @@ def is_extruder_smoothing(self, exact_mode): return not exact_mode and self.A def is_enabled(self): return self.n > 0 - def _calc_offset(self): - if not self.A: - return 0. - return sum([a * t for a, t in zip(self.A, self.T)]) / sum(self.A) def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.A, self.T = self.params.get_shaper() - self.t_offs = self._calc_offset() + self.t_offs = shaper_defs.get_shaper_offset(self.A, self.T) def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() @@ -283,7 +279,8 @@ class AxisInputSmoother: def __init__(self, params): self.params = params self.n, self.coeffs, self.smooth_time = params.get_smoother() - self.t_offs = self._calc_offset() + self.t_offs = shaper_defs.get_smoother_offset( + self.coeffs, self.smooth_time, normalized=False) self.saved_smooth_time = 0. def get_name(self): return 'smoother_' + self.axis @@ -295,18 +292,11 @@ def is_extruder_smoothing(self, exact_mode): return True def is_enabled(self): return self.smooth_time > 0. - def _calc_offset(self): - int_t0 = int_t1 = 0. - for i, c in enumerate(self.coeffs): - if i & 1: - int_t1 += c / (2**(i+1) * (i+2)) - else: - int_t0 += c / (2**i * (i+1)) - return int_t1 * self.smooth_time / int_t0 def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.coeffs, self.smooth_time = self.params.get_smoother() - self.t_offs = self._calc_offset() + self.t_offs = shaper_defs.get_smoother_offset( + self.coeffs, self.smooth_time, normalized=False) def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index fdf96eb07..00e6081db 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -62,21 +62,6 @@ def get_psd(self, axis='all'): ) -def get_shaper_offset(A, T): - return sum([a * t for a, t in zip(A, T)]) / sum(A) - -def get_smoother_offset(np, C, t_sm): - hst = t_sm * 0.5 - - n_t = 200 - t, dt = np.linspace(-hst, hst, n_t, retstep=True) - - w = np.zeros(shape=t.shape) - for c in C[::-1]: - w = w * (-t) + c - - return -np.trapz(t * w, dx=dt) - def step_response(np, t, omega, damping_ratio): t = np.maximum(t, 0.) omega = np.swapaxes(np.array(omega, ndmin=2), 0, 1) @@ -346,7 +331,7 @@ def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): A, T = shaper inv_D = 1.0 / sum(A) n = len(T) - ts = get_shaper_offset(A, T) + ts = shaper_defs.get_shaper_offset(A, T) # Calculate offset for 90 and 180 degrees turn offset_90_x = offset_90_y = offset_180 = 0. diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index 419ca6c63..9c26ac4cc 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -101,6 +101,11 @@ def get_3hump_ei_shaper(shaper_freq, damping_ratio): T = [0.0, 0.5 * t_d, t_d, 1.5 * t_d, 2.0 * t_d] return (A, T) +def get_shaper_offset(A, T): + if not A: + return 0. + return sum([a * t for a, t in zip(A, T)]) / sum(A) + def init_smoother(coeffs, smooth_time, normalize_coeffs): if not normalize_coeffs: return (list(reversed(coeffs)), smooth_time) @@ -250,6 +255,22 @@ def get_extr_si_smoother(smooth_time, normalize_coeffs=True): -2.9098248068475399,-3.1121730987848171,1.5225295066412017] return init_smoother(coeffs, smooth_time, normalize_coeffs) +def get_smoother_offset(C, t_sm, normalized=True): + int_t0 = int_t1 = 0. + if normalized: + for i, c in enumerate(C): + if i & 1: + int_t1 += c * t_sm**(i+2) / (2**(i+1) * (i+2)) + else: + int_t0 += c * t_sm**(i+1) / (2**i * (i+1)) + else: + for i, c in enumerate(C): + if i & 1: + int_t1 += c / (2**(i+1) * (i+2)) + else: + int_t0 += c / (2**i * (i+1)) + int_t1 *= t_sm + return int_t1 / int_t0 # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ diff --git a/scripts/graph_shaper.py b/scripts/graph_shaper.py index bea3feaee..a0664ac2b 100755 --- a/scripts/graph_shaper.py +++ b/scripts/graph_shaper.py @@ -153,7 +153,7 @@ def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): for s in shaper_defs.INPUT_SHAPERS: if s.name == shaper_name.lower(): A, T = s.init_func(shaper_freq, SHAPER_DAMPING_RATIO) - ts = shaper_calibrate.get_shaper_offset(A, T) + ts = shaper_defs.get_shaper_offset(A, T) T = [t - ts for t in T] shaper = A, T, s.name.upper() freqs, response, response_legend = gen_shaper_response( @@ -163,7 +163,7 @@ def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): for s in shaper_defs.INPUT_SMOOTHERS: if s.name == shaper_name.lower(): C, t_sm = s.init_func(shaper_freq) - t_offs = shaper_calibrate.get_smoother_offset(np, C, t_sm) + t_offs = shaper_defs.get_smoother_offset(C, t_sm) shaper = C, t_sm, t_offs, s.name.upper() freqs, response, response_legend = gen_shaper_response( shaper, shaper_freq, shaper_calibrate.estimate_smoother) From d5938d08b62309dbf8e8f67110cba0acfe35f8cd Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 21 Jun 2023 21:49:14 +0200 Subject: [PATCH 32/35] ringing_test: Added velocity changing options and their validation Signed-off-by: Dmitry Butyugin --- klippy/extras/ringing_test.py | 51 +++++++++++++++++++++++++++++------ 1 file changed, 43 insertions(+), 8 deletions(-) diff --git a/klippy/extras/ringing_test.py b/klippy/extras/ringing_test.py index 717210833..1f4e11194 100644 --- a/klippy/extras/ringing_test.py +++ b/klippy/extras/ringing_test.py @@ -22,8 +22,9 @@ def __init__(self, config): self.notch = config.getfloat('notch', 1., above=0.) self.notch_offset = config.getfloat('notch_offset', 0.) self.velocity = config.getfloat('velocity', 80., above=0.) + self.velocity_step = config.getfloat('velocity_step', 0.) self.accel_start = config.getfloat('accel_start', 1500., above=0.) - self.accel_step = config.getfloat('accel_step', 500., above=0.) + self.accel_step = config.getfloat('accel_step', 500.) self.center_x = config.getfloat('center_x', None) self.center_y = config.getfloat('center_y', None) self.layer_height = config.getfloat('layer_height', 0.2, above=0.) @@ -137,9 +138,10 @@ def get_gcode(self): self.notch_offset if size == self.size else size * DEFAULT_NOTCH_OFFSET_RATIO, above=2., maxval=.5*size) - velocity = gcmd.get_float('VELOCITY', self.velocity, above=0.) + velocity_start = gcmd.get_float('VELOCITY', self.velocity, above=0.) + velocity_step = gcmd.get_float('VELOCITY_STEP', self.velocity_step) accel_start = gcmd.get_float('ACCEL_START', self.accel_start, above=0.) - accel_step = gcmd.get_float('ACCEL_STEP', self.accel_step, above=0.) + accel_step = gcmd.get_float('ACCEL_STEP', self.accel_step) brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width) min_brim_width = (notch_offset - notch) / (1. + 1. / TAN_TEST_ANGLE) brim_width = max(brim_width, @@ -155,9 +157,41 @@ def get_gcode(self): inner_brim_size = inner_size - 2. * min_brim_width recipr_cos = math.sqrt(1. + TAN_TEST_ANGLE**2) - max_velocity = velocity * recipr_cos + + def get_top_velocity(): + z = first_layer_height + top_velocity = velocity_start + while z < height - 0.00000001: + band_part = math.fmod(z, band) / band + notch_pos = notch_offset - notch / (1. - math.sqrt(0.75)) * ( + 1. - math.sqrt(1. - (band_part - 0.5)**2)) + max_accel = accel_start + accel_step * math.floor(z / band) + if max_accel < .1: + msg = "All accelerations must be positive" + logging.warning(msg) + raise gcmd.error(msg) + velocity = velocity_start + velocity_step * math.floor(z / band) + if velocity < .1: + msg = "All velocities must be positive" + logging.warning(msg) + raise gcmd.error(msg) + top_velocity = max(top_velocity, velocity) + v_y = velocity * TAN_TEST_ANGLE + t_y = v_y / max_accel + d_x = velocity * t_y + min_accel_dist_x = .5 * velocity**2 / max_accel * recipr_cos + accel_dist_x = notch_pos - d_x - 1.0 - .5 * (size - inner_size) + if accel_dist_x < min_accel_dist_x: + msg = ("Too high velocity %.2f mm/sec for %.0f mm/sec^2" + " acceleration" % (velocity, max_accel)) + logging.warning(msg) + raise gcmd.error(msg) + z += layer_height + return top_velocity + max_velocity = recipr_cos * get_top_velocity() if max_velocity > old_max_velocity: yield 'SET_VELOCITY_LIMIT VELOCITY=%.3f' % (max_velocity,) + def gen_brim(): first_layer_width = line_width * FIRST_LAYER_WIDTH_MULTIPLIER extr_r = 4. * first_layer_height * first_layer_width / ( @@ -201,14 +235,13 @@ def gen_tower(): notch_pos = notch_offset - notch / (1. - math.sqrt(0.75)) * ( 1. - math.sqrt(1. - (band_part - 0.5)**2)) max_accel = accel_start + accel_step * math.floor(z / band) + velocity = velocity_start + velocity_step * math.floor(z / band) v_y = velocity * TAN_TEST_ANGLE t_y = v_y / max_accel d_y = .5 * v_y * t_y d_x = velocity * t_y notch_other_side = (notch_pos - d_x) * TAN_TEST_ANGLE + d_y perimeter_offset = .5 * inner_size - yield 'SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.3f' % ( - max_accel, .5 * max_accel) for i in range(perimeters): # Move to the start of the perimeter next_y_offset = ( @@ -229,6 +262,8 @@ def rotated_G1(x, y, e, v): center_y + notch_axis[1] * x + other_axis[1] * y, e * extr_r, v * 60.) + yield ('SET_VELOCITY_LIMIT ACCEL=%.3f ' + 'ACCEL_TO_DECEL=%.3f' % (max_accel, max_accel)) # The extrusion flow of the lines at an agle is reduced # by cos(angle) to maintain the correct spacing between # the perimeters formed by those lines @@ -237,14 +272,14 @@ def rotated_G1(x, y, e, v): perimeter_offset - d_y - TAN_TEST_ANGLE, (notch_pos - d_x - 1.0 - .5 * size + perimeter_offset), - max_velocity) + recipr_cos * velocity) yield ('SET_VELOCITY_LIMIT ACCEL=%.6f' + ' ACCEL_TO_DECEL=%.6f') % ( INFINITE_ACCEL, INFINITE_ACCEL) yield rotated_G1( notch_pos - d_x - .5 * size, perimeter_offset - d_y, - 1., max_velocity) + 1., recipr_cos * velocity) old_x, old_y = d_x, d_y for j in range(deceleration_points): x = ((deceleration_points - j - 1) * d_x From 4bbec272f1a1cae89863f8a2181f7a620b918db4 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Wed, 5 Jul 2023 21:28:56 +0200 Subject: [PATCH 33/35] shaper_calibrate: Small fix for input smoother max velocity estimation Signed-off-by: Dmitry Butyugin --- klippy/extras/shaper_calibrate.py | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index 00e6081db..6a5767a32 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -147,26 +147,29 @@ def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): test_freqs = np.asarray(test_freqs) - t_start = -hst - t_end = hst + 1.5 * np.maximum(1. / test_freqs[test_freqs > 0.], t_sm) + t_start = -t_sm + t_end = hst + np.maximum(1.5 / test_freqs[test_freqs > 0.], 2.0 * t_sm) n_t = 1000 unity_range = np.linspace(0., 1., n_t) time = (t_end[:, np.newaxis] - t_start) * unity_range + t_start dt = (time[:,-1] - time[:,0]) / n_t tau = np.copy(time) tau[time > hst] = 0. + tau[time < -hst] = 0. w = np.zeros(shape=tau.shape) for c in C[::-1]: w = w * tau + c w[time > hst] = 0. + w[time < -hst] = 0. norms = (w * dt[:, np.newaxis]).sum(axis=-1) min_v = -step_response_min_velocity(test_damping_ratio) omega = 2. * math.pi * test_freqs[test_freqs > 0.] - wl = np.count_nonzero(time <= hst, axis=-1).max() + wm = np.count_nonzero(time < -hst, axis=-1).min() + wp = np.count_nonzero(time <= hst, axis=-1).max() def get_windows(m, wl): nrows = m.shape[-1] - wl + 1 @@ -175,8 +178,8 @@ def get_windows(m, wl): strides=(m.strides[0], n, n)) s_r = step_response(np, time, omega, test_damping_ratio) - w_dt = w[:, :wl] * (np.reciprocal(norms) * dt)[:, np.newaxis] - response = np.einsum("ijk,ik->ij", get_windows(s_r, wl), w_dt[:,::-1]) + w_dt = w[:, wm:wp] * (np.reciprocal(norms) * dt)[:, np.newaxis] + response = np.einsum("ijk,ik->ij", get_windows(s_r, wp - wm), w_dt[:,::-1]) velocity = (response[:,1:] - response[:,:-1]) / (omega * dt)[:, np.newaxis] res = np.zeros(shape=test_freqs.shape) res[test_freqs > 0.] = -velocity.min(axis=-1) / min_v From af24841bdeb9ecdf6a43b7771b59a52503f6e556 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Thu, 13 Jul 2023 01:40:56 +0200 Subject: [PATCH 34/35] pa_test: Added more interesting velocity transitions Signed-off-by: Dmitry Butyugin --- klippy/extras/pa_test.py | 30 ++++++++++++++++++++---------- 1 file changed, 20 insertions(+), 10 deletions(-) diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index fd156317a..7a8c3f7c8 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -12,7 +12,7 @@ SLOW_NOTCH_SIZE = 10.0 SEAM_GAP_RATIO = 0.10 SEAM_EXTRA_WIPE_RATIO = 1.1 -VERY_SLOW_SEG = 0.01 +VERY_SLOW_SEG = 0.20 class PATest: def __init__(self, config): @@ -236,24 +236,34 @@ def gen_tower(): origin_y + .5 * SLOW_NOTCH_SIZE, (perimeter_y_offset - .5 * SLOW_NOTCH_SIZE) * extr_r, - fast_velocity * 60.) + medium_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y + .5 * VERY_SLOW_SEG, + .5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, + slow_velocity * 60.) + yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( + origin_x - perimeter_x_offset, + origin_y - .5 * VERY_SLOW_SEG, + VERY_SLOW_SEG * extr_r, + scv_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y - .5 * SLOW_NOTCH_SIZE, - SLOW_NOTCH_SIZE * extr_r, + .5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, slow_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - perimeter_x_offset, origin_y - perimeter_y_offset, (perimeter_y_offset - .5 * SLOW_NOTCH_SIZE) * extr_r, - fast_velocity * 60.) + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - x_switching_pos, origin_y - perimeter_y_offset, (perimeter_x_offset - x_switching_pos) * extr_r, fast_velocity * 60.) - yield 'G4 P0.001' + yield 'G4 P40' yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x - .5 * SLOW_NOTCH_SIZE, origin_y - perimeter_y_offset, @@ -285,24 +295,24 @@ def gen_tower(): origin_y - .5 * fast_notch_size, (perimeter_y_offset - .5 * fast_notch_size) * extr_r, - medium_velocity * 60.) + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y, .5 * fast_notch_size * extr_r, - fast_velocity * 60.) - yield 'G4 P0.001' + medium_velocity * 60.) + yield 'G4 P40' yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + .5 * fast_notch_size, .5 * fast_notch_size * extr_r, - fast_velocity * 60.) + medium_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + perimeter_x_offset, origin_y + perimeter_y_offset, (perimeter_y_offset - .5 * fast_notch_size) * extr_r, - medium_velocity * 60.) + fast_velocity * 60.) yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( origin_x + x_switching_pos, origin_y + perimeter_y_offset, From 40cc044d0400710322488c3ea7c34a6d9f430ef4 Mon Sep 17 00:00:00 2001 From: Rogerio Goncalves Date: Wed, 18 Oct 2023 11:54:57 +0100 Subject: [PATCH 35/35] python format --- README.md | 14 + klippy/chelper/__init__.py | 48 ++- klippy/extras/input_shaper.py | 460 +++++++++++++++-------- klippy/extras/motion_report.py | 38 +- klippy/extras/pa_test.py | 580 +++++++++++++++++------------ klippy/extras/ringing_test.py | 595 ++++++++++++++++++------------ klippy/extras/sdcard_loop.py | 25 +- klippy/extras/shaper_calibrate.py | 309 ++++++++++------ klippy/extras/shaper_defs.py | 323 +++++++++++----- klippy/extras/virtual_sdcard.py | 149 +++++--- klippy/kinematics/extruder.py | 290 ++++++++++----- klippy/stepper.py | 83 +++-- scripts/calibrate_shaper.py | 30 +- scripts/graph_shaper.py | 206 +++++++---- scripts/motan/readlog.py | 46 ++- 15 files changed, 2073 insertions(+), 1123 deletions(-) diff --git a/README.md b/README.md index d2036295c..d0793f508 100644 --- a/README.md +++ b/README.md @@ -38,6 +38,20 @@ Features merged into the master branch: - [homing: sensorless minimum home distance](https://github.com/DangerKlippers/danger-klipper/pull/65) +If you're feeling adventurous, take a peek at the extra features in the bleeding-edge branch: + +- [dmbutyugin's advanced-features branch](https://github.com/DangerKlippers/danger-klipper/pull/69) [dmbutyugin/advanced-features](https://github.com/dmbutyugin/klipper/commits/advanced-features) + + - [stepper: high precision stepping protocol]() + + - [extruder: sync extruder motion with input shaper]() + + - [extruder: new print_pa_tower utility]() + + - [input_shaper: smooth input shapers]() + + - [input_shaper: new print_ringing_tower utility]() + "Dangerous Klipper for dangerous users" Klipper is a 3d-Printer firmware. It combines the power of a general diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 243bbcd9f..e8943a094 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -12,23 +12,49 @@ ###################################################################### GCC_CMD = "gcc" -COMPILE_ARGS = ("-Wall -g -O3 -shared -fPIC" - " -flto -fwhole-program -fno-use-linker-plugin" - " -march=native -mcpu=native -mtune=native" - " -o %s %s") +COMPILE_ARGS = ( + "-Wall -g -O3 -shared -fPIC" + " -flto -fwhole-program -fno-use-linker-plugin" + " -march=native -mcpu=native -mtune=native" + " -o %s %s" +) SSE_FLAGS = "-mfpmath=sse -msse2" NEON_FLAGS = "-mfpu=neon" SOURCE_FILES = [ - 'pyhelper.c', 'serialqueue.c', 'stepcompress.c', 'stepcompress_hp.c', - 'itersolve.c', 'trapq.c', 'pollreactor.c', 'msgblock.c', 'trdispatch.c', - 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', - 'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', - 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'integrate.c', + "pyhelper.c", + "serialqueue.c", + "stepcompress.c", + "stepcompress_hp.c", + "itersolve.c", + "trapq.c", + "pollreactor.c", + "msgblock.c", + "trdispatch.c", + "kin_cartesian.c", + "kin_corexy.c", + "kin_corexz.c", + "kin_delta.c", + "kin_deltesian.c", + "kin_polar.c", + "kin_rotary_delta.c", + "kin_winch.c", + "kin_extruder.c", + "kin_shaper.c", + "kin_idex.c", + "integrate.c", ] DEST_LIB = "c_helper.so" OTHER_FILES = [ - 'list.h', 'serialqueue.h', 'stepcompress.h', 'itersolve.h', 'pyhelper.h', - 'trapq.h', 'pollreactor.h', 'msgblock.h', 'kin_shaper.h', 'integrate.h', + "list.h", + "serialqueue.h", + "stepcompress.h", + "itersolve.h", + "pyhelper.h", + "trapq.h", + "pollreactor.h", + "msgblock.h", + "kin_shaper.h", + "integrate.h", ] defs_stepcompress = """ diff --git a/klippy/extras/input_shaper.py b/klippy/extras/input_shaper.py index 62e344e1c..67d22172f 100644 --- a/klippy/extras/input_shaper.py +++ b/klippy/extras/input_shaper.py @@ -8,48 +8,62 @@ import chelper from . import shaper_defs + def parse_float_list(list_str): def parse_str(s): res = [] - for line in s.split('\n'): - for coeff in line.split(','): + for line in s.split("\n"): + for coeff in line.split(","): res.append(float(coeff.strip())) return res + try: return parse_str(list_str) except: return None + class TypedInputShaperParams: - shapers = {s.name : s.init_func for s in shaper_defs.INPUT_SHAPERS} + shapers = {s.name: s.init_func for s in shaper_defs.INPUT_SHAPERS} + def __init__(self, axis, shaper_type, config): self.axis = axis self.shaper_type = shaper_type self.damping_ratio = shaper_defs.DEFAULT_DAMPING_RATIO - self.shaper_freq = 0. + self.shaper_freq = 0.0 if config is not None: if shaper_type not in self.shapers: raise config.error( - 'Unsupported shaper type: %s' % (shaper_type,)) - self.damping_ratio = config.getfloat('damping_ratio_' + axis, - self.damping_ratio, - minval=0., maxval=1.) - self.shaper_freq = config.getfloat('shaper_freq_' + axis, - self.shaper_freq, minval=0.) + "Unsupported shaper type: %s" % (shaper_type,) + ) + self.damping_ratio = config.getfloat( + "damping_ratio_" + axis, + self.damping_ratio, + minval=0.0, + maxval=1.0, + ) + self.shaper_freq = config.getfloat( + "shaper_freq_" + axis, self.shaper_freq, minval=0.0 + ) + def get_type(self): return self.shaper_type + def get_axis(self): return self.axis + def update(self, shaper_type, gcmd): if shaper_type not in self.shapers: - raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) + raise gcmd.error("Unsupported shaper type: %s" % (shaper_type,)) axis = self.axis.upper() - self.damping_ratio = gcmd.get_float('DAMPING_RATIO_' + axis, - self.damping_ratio, - minval=0., maxval=1.) - self.shaper_freq = gcmd.get_float('SHAPER_FREQ_' + axis, - self.shaper_freq, minval=0.) + self.damping_ratio = gcmd.get_float( + "DAMPING_RATIO_" + axis, self.damping_ratio, minval=0.0, maxval=1.0 + ) + self.shaper_freq = gcmd.get_float( + "SHAPER_FREQ_" + axis, self.shaper_freq, minval=0.0 + ) self.shaper_type = shaper_type + def get_shaper(self): if not self.shaper_freq: A, T = shaper_defs.get_none_shaper() @@ -70,31 +84,40 @@ def get_status(self): class CustomInputShaperParams: - SHAPER_TYPE = 'custom' + SHAPER_TYPE = "custom" + def __init__(self, axis, config): self.axis = axis self.n, self.A, self.T = 0, [], [] if config is not None: - shaper_a_str = config.get('shaper_a_' + axis) - shaper_t_str = config.get('shaper_t_' + axis) + shaper_a_str = config.get("shaper_a_" + axis) + shaper_t_str = config.get("shaper_t_" + axis) self.n, self.A, self.T = self._parse_custom_shaper( - shaper_a_str, shaper_t_str, config.error) + shaper_a_str, shaper_t_str, config.error + ) + def get_type(self): return self.SHAPER_TYPE + def get_axis(self): return self.axis + def update(self, shaper_type, gcmd): if shaper_type != self.SHAPER_TYPE: - raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) + raise gcmd.error("Unsupported shaper type: %s" % (shaper_type,)) axis = self.axis.upper() - shaper_a_str = gcmd.get('SHAPER_A_' + axis, None) - shaper_t_str = gcmd.get('SHAPER_T_' + axis, None) + shaper_a_str = gcmd.get("SHAPER_A_" + axis, None) + shaper_t_str = gcmd.get("SHAPER_T_" + axis, None) if (shaper_a_str is None) != (shaper_t_str is None): - raise gcmd.error("Both SHAPER_A_%s and SHAPER_T_%s parameters" - " must be provided" % (axis, axis)) + raise gcmd.error( + "Both SHAPER_A_%s and SHAPER_T_%s parameters" + " must be provided" % (axis, axis) + ) if shaper_a_str is not None: self.n, self.A, self.T = self._parse_custom_shaper( - shaper_a_str, shaper_t_str, gcmd.error) + shaper_a_str, shaper_t_str, gcmd.error + ) + def _parse_custom_shaper(self, custom_a_str, custom_t_str, parse_error): A = parse_float_list(custom_a_str) if A is None: @@ -103,7 +126,8 @@ def _parse_custom_shaper(self, custom_a_str, custom_t_str, parse_error): raise parse_error("All shaper A coefficients must be non-zero") if sum(A) < 0.001: raise parse_error( - "Shaper A parameter must sum up to a positive number") + "Shaper A parameter must sum up to a positive number" + ) T = parse_float_list(custom_t_str) if T is None: raise parse_error("Invalid shaper T string: '%s'" % (custom_t_str,)) @@ -111,23 +135,36 @@ def _parse_custom_shaper(self, custom_a_str, custom_t_str, parse_error): raise parse_error("Shaper T parameter is not ordered: %s" % (T,)) if len(A) != len(T): raise parse_error( - "Shaper A and T parameters must have the same length:" - " %d vs %d" % (len(A), len(T),)) + "Shaper A and T parameters must have the same length:" + " %d vs %d" + % ( + len(A), + len(T), + ) + ) dur = T[-1] - T[0] if len(T) > 1 and dur < 0.001: - raise parse_error("Shaper duration is too small (%.6f sec)" - % (dur,)) + raise parse_error( + "Shaper duration is too small (%.6f sec)" % (dur,) + ) if dur > 0.2: - raise parse_error("Shaper duration is too large (%.6f sec)" - % (dur,)) + raise parse_error( + "Shaper duration is too large (%.6f sec)" % (dur,) + ) return len(A), A, T + def get_shaper(self): return self.n, self.A, self.T + def get_status(self): - return collections.OrderedDict([ - ('shaper_type', self.SHAPER_TYPE), - ('shaper_a', ','.join(['%.6f' % (a,) for a in self.A])), - ('shaper_t', ','.join(['%.6f' % (t,) for t in self.T]))]) + return collections.OrderedDict( + [ + ("shaper_type", self.SHAPER_TYPE), + ("shaper_a", ",".join(["%.6f" % (a,) for a in self.A])), + ("shaper_t", ",".join(["%.6f" % (t,) for t in self.T])), + ] + ) + class AxisInputShaper: def __init__(self, params): @@ -135,54 +172,81 @@ def __init__(self, params): self.n, self.A, self.T = params.get_shaper() self.t_offs = shaper_defs.get_shaper_offset(self.A, self.T) self.saved = None + def get_name(self): - return 'shaper_' + self.get_axis() + return "shaper_" + self.get_axis() + def get_type(self): return self.params.get_type() + def get_axis(self): return self.params.get_axis() + def is_extruder_smoothing(self, exact_mode): return not exact_mode and self.A + def is_enabled(self): return self.n > 0 + def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.A, self.T = self.params.get_shaper() self.t_offs = shaper_defs.get_shaper_offset(self.A, self.T) + def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() - success = ffi_lib.input_shaper_set_shaper_params( - sk, axis, self.n, self.A, self.T) == 0 + success = ( + ffi_lib.input_shaper_set_shaper_params( + sk, axis, self.n, self.A, self.T + ) + == 0 + ) if not success: self.disable_shaping() ffi_lib.input_shaper_set_shaper_params( - sk, axis, self.n, self.A, self.T) + sk, axis, self.n, self.A, self.T + ) return success + def update_extruder_kinematics(self, sk, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() if not self.is_extruder_smoothing(exact_mode): # Make sure to disable any active input smoothing - coeffs, smooth_time = [], 0. - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, len(coeffs), coeffs, smooth_time, 0.) == 0 - success = ffi_lib.extruder_set_shaper_params( - sk, axis, self.n, self.A, self.T) == 0 + coeffs, smooth_time = [], 0.0 + success = ( + ffi_lib.extruder_set_smoothing_params( + sk, axis, len(coeffs), coeffs, smooth_time, 0.0 + ) + == 0 + ) + success = ( + ffi_lib.extruder_set_shaper_params( + sk, axis, self.n, self.A, self.T + ) + == 0 + ) else: shaper_type = self.get_type() - extr_smoother_func = shaper_defs.EXTURDER_SMOOTHERS.get( - shaper_type, shaper_defs.EXTURDER_SMOOTHERS['default']) - C_e, t_sm = extr_smoother_func(self.T[-1] - self.T[0], - normalize_coeffs=False) + extr_smoother_func = shaper_defs.EXTRUDER_SMOOTHERS.get( + shaper_type, shaper_defs.EXTRUDER_SMOOTHERS["default"] + ) + C_e, t_sm = extr_smoother_func( + self.T[-1] - self.T[0], normalize_coeffs=False + ) smoother_offset = self.t_offs - 0.5 * (self.T[0] + self.T[-1]) - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, len(C_e), C_e, t_sm, smoother_offset) == 0 + success = ( + ffi_lib.extruder_set_smoothing_params( + sk, axis, len(C_e), C_e, t_sm, smoother_offset + ) + == 0 + ) if not success: self.disable_shaping() - ffi_lib.extruder_set_shaper_params( - sk, axis, self.n, self.A, self.T) + ffi_lib.extruder_set_shaper_params(sk, axis, self.n, self.A, self.T) return success + def disable_shaping(self): was_enabled = False if self.saved is None and self.n: @@ -191,6 +255,7 @@ def disable_shaping(self): A, T = shaper_defs.get_none_shaper() self.n, self.A, self.T = len(A), A, T return was_enabled + def enable_shaping(self): if self.saved is None: # Input shaper was not disabled @@ -198,67 +263,94 @@ def enable_shaping(self): self.n, self.A, self.T = self.saved self.saved = None return True + def report(self, gcmd): - info = ' '.join(["%s_%s:%s" % (key, self.get_axis(), value) - for (key, value) in self.params.get_status().items()]) + info = " ".join( + [ + "%s_%s:%s" % (key, self.get_axis(), value) + for (key, value) in self.params.get_status().items() + ] + ) gcmd.respond_info(info) + class TypedInputSmootherParams: - smoothers = {s.name : s.init_func for s in shaper_defs.INPUT_SMOOTHERS} + smoothers = {s.name: s.init_func for s in shaper_defs.INPUT_SMOOTHERS} + def __init__(self, axis, smoother_type, config): self.axis = axis self.smoother_type = smoother_type - self.smoother_freq = 0. + self.smoother_freq = 0.0 if config is not None: if smoother_type not in self.smoothers: raise config.error( - 'Unsupported shaper type: %s' % (smoother_type,)) - self.smoother_freq = config.getfloat('smoother_freq_' + axis, - self.smoother_freq, minval=0.) + "Unsupported shaper type: %s" % (smoother_type,) + ) + self.smoother_freq = config.getfloat( + "smoother_freq_" + axis, self.smoother_freq, minval=0.0 + ) + def get_type(self): return self.smoother_type + def get_axis(self): return self.axis + def update(self, smoother_type, gcmd): if smoother_type not in self.smoothers: - raise gcmd.error('Unsupported shaper type: %s' % (smoother_type,)) + raise gcmd.error("Unsupported shaper type: %s" % (smoother_type,)) axis = self.axis.upper() - self.smoother_freq = gcmd.get_float('SMOOTHER_FREQ_' + axis, - self.smoother_freq, minval=0.) + self.smoother_freq = gcmd.get_float( + "SMOOTHER_FREQ_" + axis, self.smoother_freq, minval=0.0 + ) self.smoother_type = smoother_type + def get_smoother(self): if not self.smoother_freq: C, tsm = shaper_defs.get_none_smoother() else: - C, tsm = self.smoothers[self.smoother_type](self.smoother_freq, - normalize_coeffs=False) + C, tsm = self.smoothers[self.smoother_type]( + self.smoother_freq, normalize_coeffs=False + ) return len(C), C, tsm + def get_status(self): - return collections.OrderedDict([ - ('shaper_type', self.smoother_type), - ('smoother_freq', '%.3f' % (self.smoother_freq,))]) + return collections.OrderedDict( + [ + ("shaper_type", self.smoother_type), + ("smoother_freq", "%.3f" % (self.smoother_freq,)), + ] + ) + class CustomInputSmootherParams: - SHAPER_TYPE = 'smoother' + SHAPER_TYPE = "smoother" + def __init__(self, axis, config): self.axis = axis self.coeffs, self.smooth_time = shaper_defs.get_none_smoother() if config is not None: - self.smooth_time = config.getfloat('smooth_time_' + axis, - self.smooth_time, minval=0.) - self.coeffs = list(reversed(config.getfloatlist('coeffs_' + axis, - self.coeffs))) + self.smooth_time = config.getfloat( + "smooth_time_" + axis, self.smooth_time, minval=0.0 + ) + self.coeffs = list( + reversed(config.getfloatlist("coeffs_" + axis, self.coeffs)) + ) + def get_type(self): return self.SHAPER_TYPE + def get_axis(self): return self.axis + def update(self, shaper_type, gcmd): if shaper_type != self.SHAPER_TYPE: - raise gcmd.error('Unsupported shaper type: %s' % (shaper_type,)) + raise gcmd.error("Unsupported shaper type: %s" % (shaper_type,)) axis = self.axis.upper() - self.smooth_time = gcmd.get_float('SMOOTH_TIME_' + axis, - self.smooth_time) - coeffs_str = gcmd.get('COEFFS_' + axis, None) + self.smooth_time = gcmd.get_float( + "SMOOTH_TIME_" + axis, self.smooth_time + ) + coeffs_str = gcmd.get("COEFFS_" + axis, None) if coeffs_str is not None: try: coeffs = parse_float_list(coeffs_str) @@ -266,47 +358,70 @@ def update(self, shaper_type, gcmd): except: raise gcmd.error("Invalid format for COEFFS parameter") self.coeffs = coeffs + def get_smoother(self): return len(self.coeffs), self.coeffs, self.smooth_time + def get_status(self): - return collections.OrderedDict([ - ('shaper_type', self.SHAPER_TYPE), - ('shaper_coeffs', ','.join(['%.9e' % (a,) - for a in reversed(self.coeffs)])), - ('shaper_smooth_time', self.smooth_time)]) + return collections.OrderedDict( + [ + ("shaper_type", self.SHAPER_TYPE), + ( + "shaper_coeffs", + ",".join(["%.9e" % (a,) for a in reversed(self.coeffs)]), + ), + ("shaper_smooth_time", self.smooth_time), + ] + ) + class AxisInputSmoother: def __init__(self, params): self.params = params self.n, self.coeffs, self.smooth_time = params.get_smoother() self.t_offs = shaper_defs.get_smoother_offset( - self.coeffs, self.smooth_time, normalized=False) - self.saved_smooth_time = 0. + self.coeffs, self.smooth_time, normalized=False + ) + self.saved_smooth_time = 0.0 + def get_name(self): - return 'smoother_' + self.axis + return "smoother_" + self.axis + def get_type(self): return self.params.get_type() + def get_axis(self): return self.params.get_axis() + def is_extruder_smoothing(self, exact_mode): return True + def is_enabled(self): - return self.smooth_time > 0. + return self.smooth_time > 0.0 + def update(self, shaper_type, gcmd): self.params.update(shaper_type, gcmd) self.n, self.coeffs, self.smooth_time = self.params.get_smoother() self.t_offs = shaper_defs.get_smoother_offset( - self.coeffs, self.smooth_time, normalized=False) + self.coeffs, self.smooth_time, normalized=False + ) + def update_stepper_kinematics(self, sk): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() - success = ffi_lib.input_shaper_set_smoother_params( - sk, axis, self.n, self.coeffs, self.smooth_time) == 0 + success = ( + ffi_lib.input_shaper_set_smoother_params( + sk, axis, self.n, self.coeffs, self.smooth_time + ) + == 0 + ) if not success: self.disable_shaping() ffi_lib.input_shaper_set_smoother_params( - sk, axis, self.n, self.coeffs, self.smooth_time) + sk, axis, self.n, self.coeffs, self.smooth_time + ) return success + def update_extruder_kinematics(self, sk, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() axis = self.get_axis().encode() @@ -314,44 +429,63 @@ def update_extruder_kinematics(self, sk, exact_mode): A, T = shaper_defs.get_none_shaper() ffi_lib.extruder_set_shaper_params(sk, axis, len(A), A, T) if exact_mode: - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, self.n, self.coeffs, self.smooth_time, - self.t_offs) == 0 + success = ( + ffi_lib.extruder_set_smoothing_params( + sk, axis, self.n, self.coeffs, self.smooth_time, self.t_offs + ) + == 0 + ) else: smoother_type = self.get_type() - extr_smoother_func = shaper_defs.EXTURDER_SMOOTHERS.get( - smoother_type, shaper_defs.EXTURDER_SMOOTHERS['default']) - C_e, t_sm = extr_smoother_func(self.smooth_time, - normalize_coeffs=False) - success = ffi_lib.extruder_set_smoothing_params( - sk, axis, len(C_e), C_e, t_sm, self.t_offs) == 0 + extr_smoother_func = shaper_defs.EXTRUDER_SMOOTHERS.get( + smoother_type, shaper_defs.EXTRUDER_SMOOTHERS["default"] + ) + C_e, t_sm = extr_smoother_func( + self.smooth_time, normalize_coeffs=False + ) + success = ( + ffi_lib.extruder_set_smoothing_params( + sk, axis, len(C_e), C_e, t_sm, self.t_offs + ) + == 0 + ) if not success: self.disable_shaping() ffi_lib.extruder_set_smoothing_params( - sk, axis, self.n, self.coeffs, self.smooth_time) + sk, axis, self.n, self.coeffs, self.smooth_time + ) return success + def disable_shaping(self): was_enabled = False if self.smooth_time: self.saved_smooth_time = self.smooth_time was_enabled = True - self.smooth_time = 0. + self.smooth_time = 0.0 return was_enabled + def enable_shaping(self): if not self.saved_smooth_time: # Input smoother was not disabled return False self.smooth_time = self.saved_smooth_time - self.saved_smooth_time = 0. + self.saved_smooth_time = 0.0 return True + def report(self, gcmd): - info = ' '.join(["%s_%s:%s" % (key, self.get_axis(), value) - for (key, value) in self.params.get_status().items()]) + info = " ".join( + [ + "%s_%s:%s" % (key, self.get_axis(), value) + for (key, value) in self.params.get_status().items() + ] + ) gcmd.respond_info(info) + class ShaperFactory: def __init__(self): pass + def _create_shaper(self, axis, type_name, config=None): if type_name == CustomInputSmootherParams.SHAPER_TYPE: return AxisInputSmoother(CustomInputSmootherParams(axis, config)) @@ -359,23 +493,28 @@ def _create_shaper(self, axis, type_name, config=None): return AxisInputShaper(CustomInputShaperParams(axis, config)) if type_name in TypedInputShaperParams.shapers: return AxisInputShaper( - TypedInputShaperParams(axis, type_name, config)) + TypedInputShaperParams(axis, type_name, config) + ) if type_name in TypedInputSmootherParams.smoothers: return AxisInputSmoother( - TypedInputSmootherParams(axis, type_name, config)) + TypedInputSmootherParams(axis, type_name, config) + ) return None + def create_shaper(self, axis, config): - shaper_type = config.get('shaper_type', 'mzv') - shaper_type = config.get('shaper_type_' + axis, shaper_type).lower() + shaper_type = config.get("shaper_type", "mzv") + shaper_type = config.get("shaper_type_" + axis, shaper_type).lower() shaper = self._create_shaper(axis, shaper_type, config) if shaper is None: raise config.error("Unsupported shaper type '%s'" % (shaper_type,)) return shaper + def update_shaper(self, shaper, gcmd): - shaper_type = gcmd.get('SHAPER_TYPE', None) + shaper_type = gcmd.get("SHAPER_TYPE", None) if shaper_type is None: - shaper_type = gcmd.get('SHAPER_TYPE_' + shaper.get_axis().upper(), - shaper.get_type()) + shaper_type = gcmd.get( + "SHAPER_TYPE_" + shaper.get_axis().upper(), shaper.get_type() + ) shaper_type = shaper_type.lower() try: shaper.update(shaper_type, gcmd) @@ -388,6 +527,7 @@ def update_shaper(self, shaper, gcmd): shaper.update(shaper_type, gcmd) return shaper + class InputShaper: def __init__(self, config): self.printer = config.get_printer() @@ -395,23 +535,32 @@ def __init__(self, config): self.toolhead = None self.extruders = [] self.exact_mode = 0 - self.config_extruder_names = config.getlist('enabled_extruders', []) + self.config_extruder_names = config.getlist("enabled_extruders", []) self.shaper_factory = ShaperFactory() - self.shapers = [self.shaper_factory.create_shaper('x', config), - self.shaper_factory.create_shaper('y', config)] + self.shapers = [ + self.shaper_factory.create_shaper("x", config), + self.shaper_factory.create_shaper("y", config), + ] self.input_shaper_stepper_kinematics = [] self.orig_stepper_kinematics = [] # Register gcode commands - gcode = self.printer.lookup_object('gcode') - gcode.register_command("SET_INPUT_SHAPER", - self.cmd_SET_INPUT_SHAPER, - desc=self.cmd_SET_INPUT_SHAPER_help) - gcode.register_command("ENABLE_INPUT_SHAPER", - self.cmd_ENABLE_INPUT_SHAPER, - desc=self.cmd_ENABLE_INPUT_SHAPER_help) - gcode.register_command("DISABLE_INPUT_SHAPER", - self.cmd_DISABLE_INPUT_SHAPER, - desc=self.cmd_DISABLE_INPUT_SHAPER_help) + gcode = self.printer.lookup_object("gcode") + gcode.register_command( + "SET_INPUT_SHAPER", + self.cmd_SET_INPUT_SHAPER, + desc=self.cmd_SET_INPUT_SHAPER_help, + ) + gcode.register_command( + "ENABLE_INPUT_SHAPER", + self.cmd_ENABLE_INPUT_SHAPER, + desc=self.cmd_ENABLE_INPUT_SHAPER_help, + ) + gcode.register_command( + "DISABLE_INPUT_SHAPER", + self.cmd_DISABLE_INPUT_SHAPER, + desc=self.cmd_DISABLE_INPUT_SHAPER_help, + ) + def get_shapers(self): return self.shapers @@ -419,9 +568,10 @@ def connect(self): self.toolhead = self.printer.lookup_object("toolhead") for en in self.config_extruder_names: extruder = self.printer.lookup_object(en) - if not hasattr(extruder, 'get_extruder_steppers'): + if not hasattr(extruder, "get_extruder_steppers"): raise self.printer.config_error( - "Invalid extruder '%s' in [input_shaper]" % (en,)) + "Invalid extruder '%s' in [input_shaper]" % (en,) + ) self.extruders.append(extruder) # Configure initial values self._update_input_shaping(error=self.printer.config_error) @@ -464,12 +614,14 @@ def _update_input_shaping(self, error=None): failed_shapers.append(shaper) new_delay = ffi_lib.input_shaper_get_step_gen_window(is_sk) if old_delay != new_delay: - self.toolhead.note_step_generation_scan_time(new_delay, - old_delay) + self.toolhead.note_step_generation_scan_time( + new_delay, old_delay + ) for e in self.extruders: for es in e.get_extruder_steppers(): - failed_shapers.extend(es.update_input_shaping(self.shapers, - self.exact_mode)) + failed_shapers.extend( + es.update_input_shaping(self.shapers, self.exact_mode) + ) if failed_shapers: error = error or self.printer.command_error raise error( @@ -491,17 +643,21 @@ def enable_shaping(self): def cmd_SET_INPUT_SHAPER(self, gcmd): if gcmd.get_command_parameters(): - self.shapers = [self.shaper_factory.update_shaper(shaper, gcmd) - for shaper in self.shapers] + self.shapers = [ + self.shaper_factory.update_shaper(shaper, gcmd) + for shaper in self.shapers + ] self._update_input_shaping() for shaper in self.shapers: shaper.report(gcmd) + cmd_ENABLE_INPUT_SHAPER_help = "Enable input shaper for given objects" + def cmd_ENABLE_INPUT_SHAPER(self, gcmd): self.toolhead.flush_step_generation() - axes = gcmd.get('AXIS', '') - msg = '' - for axis_str in axes.split(','): + axes = gcmd.get("AXIS", "") + msg = "" + for axis_str in axes.split(","): axis = axis_str.strip().lower() if not axis: continue @@ -512,16 +668,18 @@ def cmd_ENABLE_INPUT_SHAPER(self, gcmd): if s.enable_shaping(): msg += "Enabled input shaper for AXIS='%s'\n" % (axis_str,) else: - msg += ("Cannot enable input shaper for AXIS='%s': " - "was not disabled\n" % (axis_str,)) - extruders = gcmd.get('EXTRUDER', '') - self.exact_mode = gcmd.get_int('EXACT', self.exact_mode) - for en in extruders.split(','): + msg += ( + "Cannot enable input shaper for AXIS='%s': " + "was not disabled\n" % (axis_str,) + ) + extruders = gcmd.get("EXTRUDER", "") + self.exact_mode = gcmd.get_int("EXACT", self.exact_mode) + for en in extruders.split(","): extruder_name = en.strip() if not extruder_name: continue extruder = self.printer.lookup_object(extruder_name) - if not hasattr(extruder, 'get_extruder_steppers'): + if not hasattr(extruder, "get_extruder_steppers"): raise gcmd.error("Invalid EXTRUDER='%s'" % (en,)) if extruder not in self.extruders: self.extruders.append(extruder) @@ -530,12 +688,14 @@ def cmd_ENABLE_INPUT_SHAPER(self, gcmd): msg += "Input shaper already enabled for '%s'\n" % (en,) self._update_input_shaping() gcmd.respond_info(msg) + cmd_DISABLE_INPUT_SHAPER_help = "Disable input shaper for given objects" + def cmd_DISABLE_INPUT_SHAPER(self, gcmd): self.toolhead.flush_step_generation() - axes = gcmd.get('AXIS', '') - msg = '' - for axis_str in axes.split(','): + axes = gcmd.get("AXIS", "") + msg = "" + for axis_str in axes.split(","): axis = axis_str.strip().lower() if not axis: continue @@ -546,10 +706,12 @@ def cmd_DISABLE_INPUT_SHAPER(self, gcmd): if s.disable_shaping(): msg += "Disabled input shaper for AXIS='%s'\n" % (axis_str,) else: - msg += ("Cannot disable input shaper for AXIS='%s': not " - "enabled or was already disabled\n" % (axis_str,)) - extruders = gcmd.get('EXTRUDER', '') - for en in extruders.split(','): + msg += ( + "Cannot disable input shaper for AXIS='%s': not " + "enabled or was already disabled\n" % (axis_str,) + ) + extruders = gcmd.get("EXTRUDER", "") + for en in extruders.split(","): extruder_name = en.strip() if not extruder_name: continue diff --git a/klippy/extras/motion_report.py b/klippy/extras/motion_report.py index 85878bc1c..643431723 100644 --- a/klippy/extras/motion_report.py +++ b/klippy/extras/motion_report.py @@ -160,10 +160,21 @@ def log_steps(self, data): ) ) for i, s in enumerate(data): - out.append("queue_step %d: t=%d p=%d i=%d c=%d a=%d a2=%d s=%d" - % (i, s.first_clock, s.start_position, s.interval, - s.step_count, s.add, s.add2, s.shift)) - logging.info('\n'.join(out)) + out.append( + "queue_step %d: t=%d p=%d i=%d c=%d a=%d a2=%d s=%d" + % ( + i, + s.first_clock, + s.start_position, + s.interval, + s.step_count, + s.add, + s.add2, + s.shift, + ) + ) + logging.info("\n".join(out)) + def _api_update(self, eventtime): data, cdata = self.get_step_queue(self.last_api_clock, 1 << 63) if not data: @@ -180,14 +191,21 @@ def _api_update(self, eventtime): if self.mcu_stepper.get_dir_inverted()[0]: step_dist = -step_dist d = [(s.interval, s.step_count, s.add, s.add2, s.shift) for s in data] - return {"data": d, "start_position": start_position, - "start_mcu_position": mcu_pos, "step_distance": step_dist, - "first_clock": first_clock, "first_step_time": first_time, - "last_clock": last_clock, "last_step_time": last_time} + return { + "data": d, + "start_position": start_position, + "start_mcu_position": mcu_pos, + "step_distance": step_dist, + "first_clock": first_clock, + "first_step_time": first_time, + "last_clock": last_clock, + "last_step_time": last_time, + } + def _add_api_client(self, web_request): self.api_dump.add_client(web_request) - hdr = ('interval', 'count', 'add', 'add2', 'shift') - web_request.send({'header': hdr}) + hdr = ("interval", "count", "add", "add2", "shift") + web_request.send({"header": hdr}) NEVER_TIME = 9999999999999999.0 diff --git a/klippy/extras/pa_test.py b/klippy/extras/pa_test.py index 7a8c3f7c8..f40b08f03 100644 --- a/klippy/extras/pa_test.py +++ b/klippy/extras/pa_test.py @@ -14,343 +14,433 @@ SEAM_EXTRA_WIPE_RATIO = 1.1 VERY_SLOW_SEG = 0.20 + class PATest: def __init__(self, config): self.printer = config.get_printer() self.printer.register_event_handler("klippy:connect", self._connect) - self.size_x = config.getfloat('size_x', 0., minval=0.) - self.size_y = config.getfloat('size_y', 0., minval=0.) + self.size_x = config.getfloat("size_x", 0.0, minval=0.0) + self.size_y = config.getfloat("size_y", 0.0, minval=0.0) if self.size_x or self.size_y: if self.size_x < SLOW_NOTCH_SIZE * 4: raise config.error( - 'size_x must be at least %.1f' % (SLOW_NOTCH_SIZE * 4,)) + "size_x must be at least %.1f" % (SLOW_NOTCH_SIZE * 4,) + ) if self.size_y < SLOW_NOTCH_SIZE * 3: raise config.error( - 'size_y must be at least %.1f' % (SLOW_NOTCH_SIZE * 3,)) + "size_y must be at least %.1f" % (SLOW_NOTCH_SIZE * 3,) + ) max_size_y = MAX_YX_SIZE_RATIO * self.size_x if self.size_y > max_size_y: - raise config.error('Too large size_y value, maximum is %.3f' % ( - max_size_y,)) - self.height = config.getfloat('height', 50., above=0.) - self.origin_x = config.getfloat('origin_x', None) - self.origin_y = config.getfloat('origin_y', None) - self.layer_height = config.getfloat('layer_height', 0.2, above=0.) - self.first_layer_height = config.getfloat('first_layer_height', 0.2, - above=self.layer_height) - self.perimeters = config.getint('perimeters', 2, minval=1) - self.brim_width = config.getfloat('brim_width', 10., minval=2.) - self.slow_velocity = config.getfloat('slow_velocity', 25., above=0.) - self.medium_velocity = config.getfloat('medium_velocity', 50., - above=self.slow_velocity) - self.fast_velocity = config.getfloat('fast_velocity', 80., - above=self.medium_velocity) - self.filament_diameter = config.getfloat('filament_diameter', 1.75, - above=0.) + raise config.error( + "Too large size_y value, maximum is %.3f" % (max_size_y,) + ) + self.height = config.getfloat("height", 50.0, above=0.0) + self.origin_x = config.getfloat("origin_x", None) + self.origin_y = config.getfloat("origin_y", None) + self.layer_height = config.getfloat("layer_height", 0.2, above=0.0) + self.first_layer_height = config.getfloat( + "first_layer_height", 0.2, above=self.layer_height + ) + self.perimeters = config.getint("perimeters", 2, minval=1) + self.brim_width = config.getfloat("brim_width", 10.0, minval=2.0) + self.slow_velocity = config.getfloat("slow_velocity", 25.0, above=0.0) + self.medium_velocity = config.getfloat( + "medium_velocity", 50.0, above=self.slow_velocity + ) + self.fast_velocity = config.getfloat( + "fast_velocity", 80.0, above=self.medium_velocity + ) + self.filament_diameter = config.getfloat( + "filament_diameter", 1.75, above=0.0 + ) # Register commands - self.gcode = self.printer.lookup_object('gcode') - self.gcode.register_command('PRINT_PA_TOWER', - self.cmd_PRINT_PA_TOWER, - desc=self.cmd_PRINT_PA_TOWER_help) - self.progress = 0. + self.gcode = self.printer.lookup_object("gcode") + self.gcode.register_command( + "PRINT_PA_TOWER", + self.cmd_PRINT_PA_TOWER, + desc=self.cmd_PRINT_PA_TOWER_help, + ) + self.progress = 0.0 + def _connect(self): - self.sdcard = self.printer.lookup_object('virtual_sdcard', None) + self.sdcard = self.printer.lookup_object("virtual_sdcard", None) if self.sdcard is None: raise self.printer.config_error( - 'virtual_sdcard must be enabled for pa_test module to work') - toolhead = self.printer.lookup_object('toolhead') + "virtual_sdcard must be enabled for pa_test module to work" + ) + toolhead = self.printer.lookup_object("toolhead") kin_status = toolhead.get_kinematics().get_status(eventtime=None) - self.origin_x = self.origin_x or .5 * (kin_status['axis_minimum'].x + - kin_status['axis_maximum'].x) - self.origin_y = self.origin_y or .5 * (kin_status['axis_minimum'].y + - kin_status['axis_maximum'].y) + self.origin_x = self.origin_x or 0.5 * ( + kin_status["axis_minimum"].x + kin_status["axis_maximum"].x + ) + self.origin_y = self.origin_y or 0.5 * ( + kin_status["axis_minimum"].y + kin_status["axis_maximum"].y + ) if not self.size_x and not self.size_y: # Restrict the size to a maximum of 80% of available space self.size_x = min( - DEFAULT_X_SIZE, - 1.6 * (kin_status['axis_maximum'].x - self.origin_x), - 1.6 * (self.origin_x - kin_status['axis_minimum'].x)) + DEFAULT_X_SIZE, + 1.6 * (kin_status["axis_maximum"].x - self.origin_x), + 1.6 * (self.origin_x - kin_status["axis_minimum"].x), + ) self.size_y = min( - DEFAULT_Y_SIZE, - 1.6 * (kin_status['axis_maximum'].y - self.origin_y), - 1.6 * (self.origin_x - kin_status['axis_minimum'].y)) + DEFAULT_Y_SIZE, + 1.6 * (kin_status["axis_maximum"].y - self.origin_y), + 1.6 * (self.origin_x - kin_status["axis_minimum"].y), + ) + cmd_PRINT_PA_TOWER_help = "Start Pressure Advance Tower print" + def cmd_PRINT_PA_TOWER(self, gcmd): if self.is_active(): - raise gcmd.error('PA tower is already printing. If you want to ' - 'pause or cancel the print, you must enable ' - '[pause_resume] module and call a corresponding ' - 'PAUSE/CANCEL_PRINT command.') + raise gcmd.error( + "PA tower is already printing. If you want to " + "pause or cancel the print, you must enable " + "[pause_resume] module and call a corresponding " + "PAUSE/CANCEL_PRINT command." + ) self.gcmd = gcmd - self.progress = 0. + self.progress = 0.0 self.sdcard.print_with_gcode_provider(self) + def handle_shutdown(self): # Nothing to do, no resources to free pass + def get_stats(self, eventtime): if not self.is_active(): - return False, '' - return True, 'Printing PA tower' + return False, "" + return True, "Printing PA tower" + def get_status(self, eventtime): return { - 'file_path': self.get_name(), - 'progress': self.progress, - 'file_position': 0, - 'file_size': 0, + "file_path": self.get_name(), + "progress": self.progress, + "file_position": 0, + "file_size": 0, } + def is_active(self): return self.sdcard.get_gcode_provider() == self + def get_name(self): - return 'PA tower' + return "PA tower" + def reset(self): - self.progress = 0. + self.progress = 0.0 + def get_gcode(self): gcmd = self.gcmd - nozzle = gcmd.get_float('NOZZLE') + nozzle = gcmd.get_float("NOZZLE") # Check extruder temperature - target_temp = gcmd.get_float('TARGET_TEMP') - toolhead = self.printer.lookup_object('toolhead') + target_temp = gcmd.get_float("TARGET_TEMP") + toolhead = self.printer.lookup_object("toolhead") extruder_heater = toolhead.get_extruder().get_heater() systime = self.printer.get_reactor().monotonic() heater_status = extruder_heater.get_status(systime) - if target_temp != heater_status['target']: + if target_temp != heater_status["target"]: raise gcmd.error( - 'Extruder target temp %.1f does not match the expected ' - 'TARGET_TEMP=%.1f. Make sure to preheat the nozzle to ' - 'the expected temperature (e.g. with M109 gcode in a ' - 'gcode_macro prior to calling PRINT_PA_TOWER)' % ( - heater_status['target'], target_temp)) + "Extruder target temp %.1f does not match the expected " + "TARGET_TEMP=%.1f. Make sure to preheat the nozzle to " + "the expected temperature (e.g. with M109 gcode in a " + "gcode_macro prior to calling PRINT_PA_TOWER)" + % (heater_status["target"], target_temp) + ) toolhead_status = toolhead.get_status(systime) # Get tower params with overrides from the GCode command - origin_x = gcmd.get_float('ORIGIN_X', self.origin_x) - origin_y = gcmd.get_float('ORIGIN_Y', self.origin_y) - size_x = gcmd.get_float('SIZE_X', self.size_x, minval=SLOW_NOTCH_SIZE*4) - size_y = gcmd.get_float('SIZE_Y', self.size_y, minval=SLOW_NOTCH_SIZE*3, - maxval=MAX_YX_SIZE_RATIO*size_x) - perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) - slow_velocity = gcmd.get_float('SLOW_VELOCITY', self.slow_velocity, - above=0.) - medium_velocity = gcmd.get_float('MEDIUM_VELOCITY', - self.medium_velocity, - above=slow_velocity) - fast_velocity = gcmd.get_float('FAST_VELOCITY', self.fast_velocity, - above=medium_velocity) - scv_velocity = gcmd.get_float('SCV_VELOCITY', - toolhead_status['square_corner_velocity'], - above=0.) - filament_diameter = gcmd.get_float('FILAMENT_DIAMETER', - self.filament_diameter, above=0.) - layer_height = gcmd.get_float('LAYER_HEIGHT', - self.layer_height, above=0.) - first_layer_height = gcmd.get_float('FIRST_LAYER_HEIGHT', - self.first_layer_height, - above=layer_height) - height = gcmd.get_float('HEIGHT', self.height, above=0.) - step_height = gcmd.get_float('STEP_HEIGHT', 0., minval=0.) - brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width, above=nozzle) - final_gcode_id = gcmd.get('FINAL_GCODE_ID', None) + origin_x = gcmd.get_float("ORIGIN_X", self.origin_x) + origin_y = gcmd.get_float("ORIGIN_Y", self.origin_y) + size_x = gcmd.get_float( + "SIZE_X", self.size_x, minval=SLOW_NOTCH_SIZE * 4 + ) + size_y = gcmd.get_float( + "SIZE_Y", + self.size_y, + minval=SLOW_NOTCH_SIZE * 3, + maxval=MAX_YX_SIZE_RATIO * size_x, + ) + perimeters = gcmd.get_int("PERIMETERS", self.perimeters, minval=1) + slow_velocity = gcmd.get_float( + "SLOW_VELOCITY", self.slow_velocity, above=0.0 + ) + medium_velocity = gcmd.get_float( + "MEDIUM_VELOCITY", self.medium_velocity, above=slow_velocity + ) + fast_velocity = gcmd.get_float( + "FAST_VELOCITY", self.fast_velocity, above=medium_velocity + ) + scv_velocity = gcmd.get_float( + "SCV_VELOCITY", toolhead_status["square_corner_velocity"], above=0.0 + ) + filament_diameter = gcmd.get_float( + "FILAMENT_DIAMETER", self.filament_diameter, above=0.0 + ) + layer_height = gcmd.get_float( + "LAYER_HEIGHT", self.layer_height, above=0.0 + ) + first_layer_height = gcmd.get_float( + "FIRST_LAYER_HEIGHT", self.first_layer_height, above=layer_height + ) + height = gcmd.get_float("HEIGHT", self.height, above=0.0) + step_height = gcmd.get_float("STEP_HEIGHT", 0.0, minval=0.0) + brim_width = gcmd.get_float("BRIM_WIDTH", self.brim_width, above=nozzle) + final_gcode_id = gcmd.get("FINAL_GCODE_ID", None) - logging.info("Starting PA tower print of size %.3f x %.3f x %.3f mm" - " at (%.3f,%.3f)" % (size_x, size_y, height, - origin_x, origin_y)) + logging.info( + "Starting PA tower print of size %.3f x %.3f x %.3f mm" + " at (%.3f,%.3f)" % (size_x, size_y, height, origin_x, origin_y) + ) inner_size_x = size_x - 2 * nozzle * perimeters inner_size_y = size_y - 2 * nozzle * perimeters def gen_brim(): first_layer_width = nozzle * FIRST_LAYER_WIDTH_MULTIPLIER - extr_r = 4. * first_layer_height * first_layer_width / ( - math.pi * filament_diameter**2) - brim_x_offset = .5 * inner_size_x + brim_width - brim_y_offset = .5 * inner_size_y + brim_width + extr_r = ( + 4.0 + * first_layer_height + * first_layer_width + / (math.pi * filament_diameter**2) + ) + brim_x_offset = 0.5 * inner_size_x + brim_width + brim_y_offset = 0.5 * inner_size_y + brim_width start_x = origin_x - brim_x_offset start_y = origin_y - brim_y_offset start_z = first_layer_height - yield 'G1 X%.3f Y%.3f Z%.3f F%.f' % ( - start_x, start_y, start_z, fast_velocity * 60.) - while brim_x_offset > .5 * (inner_size_x - first_layer_width): - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - brim_x_offset, origin_y + brim_y_offset, - 2. * brim_y_offset * extr_r, slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + brim_x_offset, origin_y + brim_y_offset, - 2. * brim_x_offset * extr_r, slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + brim_x_offset, origin_y - brim_y_offset, - 2. * brim_y_offset * extr_r, slow_velocity * 60.) + yield "G1 X%.3f Y%.3f Z%.3f F%.f" % ( + start_x, + start_y, + start_z, + fast_velocity * 60.0, + ) + while brim_x_offset > 0.5 * (inner_size_x - first_layer_width): + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - brim_x_offset, + origin_y + brim_y_offset, + 2.0 * brim_y_offset * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + brim_x_offset, + origin_y + brim_y_offset, + 2.0 * brim_x_offset * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + brim_x_offset, + origin_y - brim_y_offset, + 2.0 * brim_y_offset * extr_r, + slow_velocity * 60.0, + ) new_brim_x_offset = brim_x_offset - first_layer_width new_brim_y_offset = brim_y_offset - first_layer_width - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - new_brim_x_offset, - origin_y - brim_y_offset, - (brim_x_offset + new_brim_x_offset) * extr_r, - slow_velocity * 60.) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - new_brim_x_offset, + origin_y - brim_y_offset, + (brim_x_offset + new_brim_x_offset) * extr_r, + slow_velocity * 60.0, + ) brim_x_offset = new_brim_x_offset brim_y_offset = new_brim_y_offset - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - brim_x_offset, origin_y + brim_y_offset, - 2. * brim_y_offset * extr_r, slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x, origin_y + brim_y_offset, - brim_x_offset * extr_r, slow_velocity * 60.) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - brim_x_offset, + origin_y + brim_y_offset, + 2.0 * brim_y_offset * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x, + origin_y + brim_y_offset, + brim_x_offset * extr_r, + slow_velocity * 60.0, + ) self.progress = start_z / height + def gen_tower(): last_z = first_layer_height z = first_layer_height + layer_height - x_switching_pos = size_x / 3. - fast_notch_size = size_y - 2. * SLOW_NOTCH_SIZE + x_switching_pos = size_x / 3.0 + fast_notch_size = size_y - 2.0 * SLOW_NOTCH_SIZE while z < height - 0.00000001: line_width = nozzle - perimeter_x_offset = .5 * inner_size_x - perimeter_y_offset = .5 * inner_size_y - if (step_height and math.floor(z / step_height) > - math.floor(last_z / step_height)): + perimeter_x_offset = 0.5 * inner_size_x + perimeter_y_offset = 0.5 * inner_size_y + if step_height and math.floor(z / step_height) > math.floor( + last_z / step_height + ): # Generate a bit thicker wall for better visual separation # of bands with different values of Pressure Advance params line_width += 0.6 * nozzle / perimeters - extra_offs = .5 * (line_width - nozzle) * (perimeters - 1) + extra_offs = 0.5 * (line_width - nozzle) * (perimeters - 1) perimeter_x_offset -= extra_offs perimeter_y_offset -= extra_offs - extr_r = 4. * layer_height * line_width / ( - math.pi * filament_diameter**2) + extr_r = ( + 4.0 + * layer_height + * line_width + / (math.pi * filament_diameter**2) + ) # Move to the start of the perimeter - yield 'G1 X%.3f Y%.3f F%.f' % ( - origin_x, origin_y + perimeter_y_offset, - fast_velocity * 60.) - ## Move the nozzle up - yield 'G1 Z%.6f F%.f' % (z, slow_velocity * 60.) + yield "G1 X%.3f Y%.3f F%.f" % ( + origin_x, + origin_y + perimeter_y_offset, + fast_velocity * 60.0, + ) + # Move the nozzle up + yield "G1 Z%.6f F%.f" % (z, slow_velocity * 60.0) for i in range(perimeters): # Print the perimiter loop alternating velocities - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - x_switching_pos, - origin_y + perimeter_y_offset, - x_switching_pos * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y + perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, - medium_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y + .5 * SLOW_NOTCH_SIZE, - (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, - medium_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y + .5 * VERY_SLOW_SEG, - .5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, - slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y - .5 * VERY_SLOW_SEG, - VERY_SLOW_SEG * extr_r, - scv_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y - .5 * SLOW_NOTCH_SIZE, - .5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, - slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - perimeter_x_offset, - origin_y - perimeter_y_offset, - (perimeter_y_offset - - .5 * SLOW_NOTCH_SIZE) * extr_r, - medium_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - x_switching_pos, - origin_y - perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, - fast_velocity * 60.) - yield 'G4 P40' - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x - .5 * SLOW_NOTCH_SIZE, - origin_y - perimeter_y_offset, - (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + .5 * SLOW_NOTCH_SIZE, - origin_y - perimeter_y_offset, - SLOW_NOTCH_SIZE * extr_r, - slow_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + x_switching_pos, - origin_y - perimeter_y_offset, - (x_switching_pos - .5 * SLOW_NOTCH_SIZE) * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + x_switching_pos + VERY_SLOW_SEG, - origin_y - perimeter_y_offset, - VERY_SLOW_SEG * extr_r, - scv_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, - origin_y - perimeter_y_offset, - (perimeter_x_offset - x_switching_pos - - VERY_SLOW_SEG) * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, - origin_y - .5 * fast_notch_size, - (perimeter_y_offset - - .5 * fast_notch_size) * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, - origin_y, - .5 * fast_notch_size * extr_r, - medium_velocity * 60.) - yield 'G4 P40' - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, - origin_y + .5 * fast_notch_size, - .5 * fast_notch_size * extr_r, - medium_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + perimeter_x_offset, - origin_y + perimeter_y_offset, - (perimeter_y_offset - - .5 * fast_notch_size) * extr_r, - fast_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + x_switching_pos, - origin_y + perimeter_y_offset, - (perimeter_x_offset - x_switching_pos) * extr_r, - medium_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - origin_x + nozzle * SEAM_GAP_RATIO, - origin_y + perimeter_y_offset, - (x_switching_pos - - nozzle * SEAM_GAP_RATIO) * extr_r, - fast_velocity * 60.) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - x_switching_pos, + origin_y + perimeter_y_offset, + x_switching_pos * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + medium_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y + 0.5 * SLOW_NOTCH_SIZE, + (perimeter_y_offset - 0.5 * SLOW_NOTCH_SIZE) * extr_r, + medium_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y + 0.5 * VERY_SLOW_SEG, + 0.5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y - 0.5 * VERY_SLOW_SEG, + VERY_SLOW_SEG * extr_r, + scv_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y - 0.5 * SLOW_NOTCH_SIZE, + 0.5 * (SLOW_NOTCH_SIZE - VERY_SLOW_SEG) * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - perimeter_x_offset, + origin_y - perimeter_y_offset, + (perimeter_y_offset - 0.5 * SLOW_NOTCH_SIZE) * extr_r, + medium_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - x_switching_pos, + origin_y - perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + fast_velocity * 60.0, + ) + yield "G4 P40" + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x - 0.5 * SLOW_NOTCH_SIZE, + origin_y - perimeter_y_offset, + (x_switching_pos - 0.5 * SLOW_NOTCH_SIZE) * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + 0.5 * SLOW_NOTCH_SIZE, + origin_y - perimeter_y_offset, + SLOW_NOTCH_SIZE * extr_r, + slow_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + x_switching_pos, + origin_y - perimeter_y_offset, + (x_switching_pos - 0.5 * SLOW_NOTCH_SIZE) * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + x_switching_pos + VERY_SLOW_SEG, + origin_y - perimeter_y_offset, + VERY_SLOW_SEG * extr_r, + scv_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + perimeter_x_offset, + origin_y - perimeter_y_offset, + (perimeter_x_offset - x_switching_pos - VERY_SLOW_SEG) + * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + perimeter_x_offset, + origin_y - 0.5 * fast_notch_size, + (perimeter_y_offset - 0.5 * fast_notch_size) * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + perimeter_x_offset, + origin_y, + 0.5 * fast_notch_size * extr_r, + medium_velocity * 60.0, + ) + yield "G4 P40" + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + perimeter_x_offset, + origin_y + 0.5 * fast_notch_size, + 0.5 * fast_notch_size * extr_r, + medium_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + perimeter_x_offset, + origin_y + perimeter_y_offset, + (perimeter_y_offset - 0.5 * fast_notch_size) * extr_r, + fast_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + x_switching_pos, + origin_y + perimeter_y_offset, + (perimeter_x_offset - x_switching_pos) * extr_r, + medium_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + origin_x + nozzle * SEAM_GAP_RATIO, + origin_y + perimeter_y_offset, + (x_switching_pos - nozzle * SEAM_GAP_RATIO) * extr_r, + fast_velocity * 60.0, + ) if i < perimeters - 1: # Switch to the next perimeter perimeter_x_offset += line_width perimeter_y_offset += line_width - yield 'G1 X%.3f Y%.3f F%.f' % ( - origin_x, origin_y + perimeter_y_offset, - fast_velocity * 60.) + yield "G1 X%.3f Y%.3f F%.f" % ( + origin_x, + origin_y + perimeter_y_offset, + fast_velocity * 60.0, + ) else: # Hide the seam a bit - yield 'G1 X%.3f Y%.3f F%.f' % ( - origin_x - nozzle * SEAM_EXTRA_WIPE_RATIO, - origin_y + perimeter_y_offset, - fast_velocity * 60.) + yield "G1 X%.3f Y%.3f F%.f" % ( + origin_x - nozzle * SEAM_EXTRA_WIPE_RATIO, + origin_y + perimeter_y_offset, + fast_velocity * 60.0, + ) self.progress = z / height last_z = z z += layer_height - yield 'M83' - yield 'G90' + yield "M83" + yield "G90" for line in gen_brim(): yield line for line in gen_tower(): yield line if final_gcode_id is not None: yield "UPDATE_DELAYED_GCODE ID='%s' DURATION=0.01" % ( - final_gcode_id,) + final_gcode_id, + ) self.progress = 1.0 + def load_config(config): return PATest(config) diff --git a/klippy/extras/ringing_test.py b/klippy/extras/ringing_test.py index 1f4e11194..48f4beb70 100644 --- a/klippy/extras/ringing_test.py +++ b/klippy/extras/ringing_test.py @@ -7,171 +7,205 @@ FIRST_LAYER_WIDTH_MULTIPLIER = 1.1 DEFAULT_SIZE = 100.0 -TAN_TEST_ANGLE = 1. -DEFAULT_NOTCH_OFFSET_RATIO = .275 +TAN_TEST_ANGLE = 1.0 +DEFAULT_NOTCH_OFFSET_RATIO = 0.275 INFINITE_ACCEL = 1000000.0 LETTER_BAND_PART = 0.82 + class RingingTest: def __init__(self, config): self.printer = config.get_printer() self.printer.register_event_handler("klippy:connect", self._connect) - self.size = config.getfloat('size', 0., minval=50.) - self.height = config.getfloat('height', 60., above=0.) - self.band = config.getfloat('band', 5., above=0.) - self.notch = config.getfloat('notch', 1., above=0.) - self.notch_offset = config.getfloat('notch_offset', 0.) - self.velocity = config.getfloat('velocity', 80., above=0.) - self.velocity_step = config.getfloat('velocity_step', 0.) - self.accel_start = config.getfloat('accel_start', 1500., above=0.) - self.accel_step = config.getfloat('accel_step', 500.) - self.center_x = config.getfloat('center_x', None) - self.center_y = config.getfloat('center_y', None) - self.layer_height = config.getfloat('layer_height', 0.2, above=0.) - self.first_layer_height = config.getfloat('first_layer_height', 0.2, - above=self.layer_height) - self.perimeters = config.getint('perimeters', 2, minval=1) - self.brim_width = config.getfloat('brim_width', 10., - minval=self.notch+2.) - self.brim_velocity = config.getfloat('brim_velocity', 30., above=0.) - self.filament_diameter = config.getfloat('filament_diameter', 1.75, - above=0.) - self.deceleration_points = config.getint('deceleration_points', 100, - minval=10) + self.size = config.getfloat("size", 0.0, minval=50.0) + self.height = config.getfloat("height", 60.0, above=0.0) + self.band = config.getfloat("band", 5.0, above=0.0) + self.notch = config.getfloat("notch", 1.0, above=0.0) + self.notch_offset = config.getfloat("notch_offset", 0.0) + self.velocity = config.getfloat("velocity", 80.0, above=0.0) + self.velocity_step = config.getfloat("velocity_step", 0.0) + self.accel_start = config.getfloat("accel_start", 1500.0, above=0.0) + self.accel_step = config.getfloat("accel_step", 500.0) + self.center_x = config.getfloat("center_x", None) + self.center_y = config.getfloat("center_y", None) + self.layer_height = config.getfloat("layer_height", 0.2, above=0.0) + self.first_layer_height = config.getfloat( + "first_layer_height", 0.2, above=self.layer_height + ) + self.perimeters = config.getint("perimeters", 2, minval=1) + self.brim_width = config.getfloat( + "brim_width", 10.0, minval=self.notch + 2.0 + ) + self.brim_velocity = config.getfloat("brim_velocity", 30.0, above=0.0) + self.filament_diameter = config.getfloat( + "filament_diameter", 1.75, above=0.0 + ) + self.deceleration_points = config.getint( + "deceleration_points", 100, minval=10 + ) # Register commands - self.gcode = self.printer.lookup_object('gcode') - self.gcode.register_command('PRINT_RINGING_TOWER', - self.cmd_PRINT_RINGING_TOWER, - desc=self.cmd_PRINT_RINGING_TOWER_help) - self.progress = 0. + self.gcode = self.printer.lookup_object("gcode") + self.gcode.register_command( + "PRINT_RINGING_TOWER", + self.cmd_PRINT_RINGING_TOWER, + desc=self.cmd_PRINT_RINGING_TOWER_help, + ) + self.progress = 0.0 + def _connect(self): - self.sdcard = self.printer.lookup_object('virtual_sdcard', None) + self.sdcard = self.printer.lookup_object("virtual_sdcard", None) if self.sdcard is None: raise self.printer.config_error( - 'virtual_sdcard must be enabled for pa_test module to work') - toolhead = self.printer.lookup_object('toolhead') + "virtual_sdcard must be enabled for pa_test module to work" + ) + toolhead = self.printer.lookup_object("toolhead") kin_status = toolhead.get_kinematics().get_status(eventtime=None) - self.center_x = self.center_x or .5 * (kin_status['axis_minimum'].x + - kin_status['axis_maximum'].x) - self.center_y = self.center_y or .5 * (kin_status['axis_minimum'].y + - kin_status['axis_maximum'].y) + self.center_x = self.center_x or 0.5 * ( + kin_status["axis_minimum"].x + kin_status["axis_maximum"].x + ) + self.center_y = self.center_y or 0.5 * ( + kin_status["axis_minimum"].y + kin_status["axis_maximum"].y + ) if not self.size: # Restrict the size to a maximum of 80% of available space self.size = min( - DEFAULT_SIZE, - 1.6 * (kin_status['axis_maximum'].x - self.center_x), - 1.6 * (self.center_x - kin_status['axis_minimum'].x)) + DEFAULT_SIZE, + 1.6 * (kin_status["axis_maximum"].x - self.center_x), + 1.6 * (self.center_x - kin_status["axis_minimum"].x), + ) if self.notch_offset: raise self.printer.config_error( - 'notch_offset must not be set if size is not provided') + "notch_offset must not be set if size is not provided" + ) self.notch_offset = self.size * DEFAULT_NOTCH_OFFSET_RATIO else: if not self.notch_offset: self.notch_offset = self.size * DEFAULT_NOTCH_OFFSET_RATIO - if self.notch_offset < 2. or self.notch_offset > .5 * self.size: + if self.notch_offset < 2.0 or self.notch_offset > 0.5 * self.size: raise self.printer.config_error( - 'notch_offset must not be in range [2.0, %.3f]' % ( - .5 * self.size,)) + "notch_offset must not be in range [2.0, %.3f]" + % (0.5 * self.size,) + ) cmd_PRINT_RINGING_TOWER_help = "Start Ringing Tower print" + def cmd_PRINT_RINGING_TOWER(self, gcmd): if self.is_active(): - raise gcmd.error('Ringing tower is already printing. If you want ' - 'to pause or cancel the print, you must enable ' - '[pause_resume] module and call a corresponding ' - 'PAUSE/CANCEL_PRINT command.') + raise gcmd.error( + "Ringing tower is already printing. If you want " + "to pause or cancel the print, you must enable " + "[pause_resume] module and call a corresponding " + "PAUSE/CANCEL_PRINT command." + ) self.gcmd = gcmd - self.progress = 0. + self.progress = 0.0 self.sdcard.print_with_gcode_provider(self) + def handle_shutdown(self): # Nothing to do, no resources to free pass + def get_stats(self, eventtime): if not self.is_active(): - return False, '' - return True, 'Printing ringing tower' + return False, "" + return True, "Printing ringing tower" + def get_status(self, eventtime): return { - 'file_path': self.get_name(), - 'progress': self.progress, - 'file_position': 0, - 'file_size': 0, + "file_path": self.get_name(), + "progress": self.progress, + "file_position": 0, + "file_size": 0, } + def is_active(self): return self.sdcard.get_gcode_provider() == self + def get_name(self): - return 'Ringing tower' + return "Ringing tower" + def reset(self): - self.progress = 0. + self.progress = 0.0 + def get_gcode(self): gcmd = self.gcmd - nozzle = gcmd.get_float('NOZZLE') + nozzle = gcmd.get_float("NOZZLE") line_width = nozzle * 1.2 # Get velocity and acceleration limits - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") systime = self.printer.get_reactor().monotonic() toolhead_info = toolhead.get_status(systime) - old_max_accel = toolhead_info['max_accel'] - old_max_accel_to_decel = toolhead_info['max_accel_to_decel'] - old_max_velocity = toolhead_info['max_velocity'] + old_max_accel = toolhead_info["max_accel"] + old_max_accel_to_decel = toolhead_info["max_accel_to_decel"] + old_max_velocity = toolhead_info["max_velocity"] # Get tower params with overrides from the GCode command - center_x = gcmd.get_float('CENTER_X', self.center_x) - center_y = gcmd.get_float('CENTER_Y', self.center_y) - size = gcmd.get_float('SIZE', self.size, minval=50.) - perimeters = gcmd.get_int('PERIMETERS', self.perimeters, minval=1) - brim_velocity = gcmd.get_float('BRIM_VELOCITY', self.brim_velocity, - above=0.) - filament_diameter = gcmd.get_float('FILAMENT_DIAMETER', - self.filament_diameter, above=0.) - layer_height = gcmd.get_float('LAYER_HEIGHT', - self.layer_height, above=0.) - first_layer_height = gcmd.get_float('FIRST_LAYER_HEIGHT', - self.first_layer_height, - above=layer_height) - height = gcmd.get_float('HEIGHT', self.height, above=0.) - band = gcmd.get_float('BAND', self.band, above=0.) - notch = gcmd.get_float('NOTCH', self.notch, above=0.) + center_x = gcmd.get_float("CENTER_X", self.center_x) + center_y = gcmd.get_float("CENTER_Y", self.center_y) + size = gcmd.get_float("SIZE", self.size, minval=50.0) + perimeters = gcmd.get_int("PERIMETERS", self.perimeters, minval=1) + brim_velocity = gcmd.get_float( + "BRIM_VELOCITY", self.brim_velocity, above=0.0 + ) + filament_diameter = gcmd.get_float( + "FILAMENT_DIAMETER", self.filament_diameter, above=0.0 + ) + layer_height = gcmd.get_float( + "LAYER_HEIGHT", self.layer_height, above=0.0 + ) + first_layer_height = gcmd.get_float( + "FIRST_LAYER_HEIGHT", self.first_layer_height, above=layer_height + ) + height = gcmd.get_float("HEIGHT", self.height, above=0.0) + band = gcmd.get_float("BAND", self.band, above=0.0) + notch = gcmd.get_float("NOTCH", self.notch, above=0.0) notch_offset = gcmd.get_float( - 'NOTCH_OFFSET', - self.notch_offset if size == self.size - else size * DEFAULT_NOTCH_OFFSET_RATIO, - above=2., maxval=.5*size) - velocity_start = gcmd.get_float('VELOCITY', self.velocity, above=0.) - velocity_step = gcmd.get_float('VELOCITY_STEP', self.velocity_step) - accel_start = gcmd.get_float('ACCEL_START', self.accel_start, above=0.) - accel_step = gcmd.get_float('ACCEL_STEP', self.accel_step) - brim_width = gcmd.get_float('BRIM_WIDTH', self.brim_width) - min_brim_width = (notch_offset - notch) / (1. + 1. / TAN_TEST_ANGLE) - brim_width = max(brim_width, - min_brim_width + perimeters * line_width + 1.) - final_gcode_id = gcmd.get('FINAL_GCODE_ID', None) - deceleration_points = gcmd.get_int('DECELERATION_POINTS', - self.deceleration_points, minval=10) + "NOTCH_OFFSET", + self.notch_offset + if size == self.size + else size * DEFAULT_NOTCH_OFFSET_RATIO, + above=2.0, + maxval=0.5 * size, + ) + velocity_start = gcmd.get_float("VELOCITY", self.velocity, above=0.0) + velocity_step = gcmd.get_float("VELOCITY_STEP", self.velocity_step) + accel_start = gcmd.get_float("ACCEL_START", self.accel_start, above=0.0) + accel_step = gcmd.get_float("ACCEL_STEP", self.accel_step) + brim_width = gcmd.get_float("BRIM_WIDTH", self.brim_width) + min_brim_width = (notch_offset - notch) / (1.0 + 1.0 / TAN_TEST_ANGLE) + brim_width = max( + brim_width, min_brim_width + perimeters * line_width + 1.0 + ) + final_gcode_id = gcmd.get("FINAL_GCODE_ID", None) + deceleration_points = gcmd.get_int( + "DECELERATION_POINTS", self.deceleration_points, minval=10 + ) - logging.info("Starting ringing tower print of size %.3fx%.3fx%.3f mm" - " at (%.3f,%.3f)" % (size, size, height, - center_x, center_y)) + logging.info( + "Starting ringing tower print of size %.3fx%.3fx%.3f mm" + " at (%.3f,%.3f)" % (size, size, height, center_x, center_y) + ) inner_size = size - 2 * (line_width * perimeters + notch) - inner_brim_size = inner_size - 2. * min_brim_width + inner_brim_size = inner_size - 2.0 * min_brim_width - recipr_cos = math.sqrt(1. + TAN_TEST_ANGLE**2) + recipr_cos = math.sqrt(1.0 + TAN_TEST_ANGLE**2) def get_top_velocity(): z = first_layer_height top_velocity = velocity_start while z < height - 0.00000001: band_part = math.fmod(z, band) / band - notch_pos = notch_offset - notch / (1. - math.sqrt(0.75)) * ( - 1. - math.sqrt(1. - (band_part - 0.5)**2)) + notch_pos = notch_offset - notch / (1.0 - math.sqrt(0.75)) * ( + 1.0 - math.sqrt(1.0 - (band_part - 0.5) ** 2) + ) max_accel = accel_start + accel_step * math.floor(z / band) - if max_accel < .1: + if max_accel < 0.1: msg = "All accelerations must be positive" logging.warning(msg) raise gcmd.error(msg) velocity = velocity_start + velocity_step * math.floor(z / band) - if velocity < .1: + if velocity < 0.1: msg = "All velocities must be positive" logging.warning(msg) raise gcmd.error(msg) @@ -179,214 +213,313 @@ def get_top_velocity(): v_y = velocity * TAN_TEST_ANGLE t_y = v_y / max_accel d_x = velocity * t_y - min_accel_dist_x = .5 * velocity**2 / max_accel * recipr_cos - accel_dist_x = notch_pos - d_x - 1.0 - .5 * (size - inner_size) + min_accel_dist_x = 0.5 * velocity**2 / max_accel * recipr_cos + accel_dist_x = notch_pos - d_x - 1.0 - 0.5 * (size - inner_size) if accel_dist_x < min_accel_dist_x: - msg = ("Too high velocity %.2f mm/sec for %.0f mm/sec^2" - " acceleration" % (velocity, max_accel)) + msg = ( + "Too high velocity %.2f mm/sec for %.0f mm/sec^2" + " acceleration" % (velocity, max_accel) + ) logging.warning(msg) raise gcmd.error(msg) z += layer_height return top_velocity + max_velocity = recipr_cos * get_top_velocity() if max_velocity > old_max_velocity: - yield 'SET_VELOCITY_LIMIT VELOCITY=%.3f' % (max_velocity,) + yield "SET_VELOCITY_LIMIT VELOCITY=%.3f" % (max_velocity,) def gen_brim(): first_layer_width = line_width * FIRST_LAYER_WIDTH_MULTIPLIER - extr_r = 4. * first_layer_height * first_layer_width / ( - math.pi * filament_diameter**2) - brim_offset = .5 * inner_brim_size + brim_width + extr_r = ( + 4.0 + * first_layer_height + * first_layer_width + / (math.pi * filament_diameter**2) + ) + brim_offset = 0.5 * inner_brim_size + brim_width start_x = center_x - brim_offset start_y = center_y - brim_offset start_z = first_layer_height - yield 'SET_VELOCITY_LIMIT ACCEL=%.6f ACCEL_TO_DECEL=%.6f' % ( - accel_start, .5 * accel_start) - yield 'G1 X%.3f Y%.3f Z%.3f F%.f' % ( - start_x, start_y, start_z, - max_velocity * 60.) - while brim_offset > .5 * inner_brim_size: - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - center_x - brim_offset, center_y + brim_offset, - 2. * brim_offset * extr_r, brim_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - center_x + brim_offset, center_y + brim_offset, - 2. * brim_offset * extr_r, brim_velocity * 60.) - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - center_x + brim_offset, center_y - brim_offset, - 2. * brim_offset * extr_r, brim_velocity * 60.) + yield "SET_VELOCITY_LIMIT ACCEL=%.6f ACCEL_TO_DECEL=%.6f" % ( + accel_start, + 0.5 * accel_start, + ) + yield "G1 X%.3f Y%.3f Z%.3f F%.f" % ( + start_x, + start_y, + start_z, + max_velocity * 60.0, + ) + while brim_offset > 0.5 * inner_brim_size: + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + center_x - brim_offset, + center_y + brim_offset, + 2.0 * brim_offset * extr_r, + brim_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + center_x + brim_offset, + center_y + brim_offset, + 2.0 * brim_offset * extr_r, + brim_velocity * 60.0, + ) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + center_x + brim_offset, + center_y - brim_offset, + 2.0 * brim_offset * extr_r, + brim_velocity * 60.0, + ) new_brim_offset = brim_offset - first_layer_width - yield 'G1 X%.3f Y%.3f E%.6f F%.f' % ( - center_x - new_brim_offset, - center_y - brim_offset, - (brim_offset + new_brim_offset) * extr_r, - brim_velocity * 60.) + yield "G1 X%.3f Y%.3f E%.6f F%.f" % ( + center_x - new_brim_offset, + center_y - brim_offset, + (brim_offset + new_brim_offset) * extr_r, + brim_velocity * 60.0, + ) brim_offset = new_brim_offset self.progress = start_z / height + def gen_tower(): prev_z = first_layer_height z = first_layer_height + layer_height - extr_r = 4. * layer_height * line_width / ( - math.pi * filament_diameter**2) - letter_offset = .5 * (size - band) - notch_offset * TAN_TEST_ANGLE + extr_r = ( + 4.0 + * layer_height + * line_width + / (math.pi * filament_diameter**2) + ) + letter_offset = 0.5 * (size - band) - notch_offset * TAN_TEST_ANGLE while z < height - 0.00000001: next_z = z + layer_height band_part = math.fmod(z, band) / band - notch_pos = notch_offset - notch / (1. - math.sqrt(0.75)) * ( - 1. - math.sqrt(1. - (band_part - 0.5)**2)) + notch_pos = notch_offset - notch / (1.0 - math.sqrt(0.75)) * ( + 1.0 - math.sqrt(1.0 - (band_part - 0.5) ** 2) + ) max_accel = accel_start + accel_step * math.floor(z / band) velocity = velocity_start + velocity_step * math.floor(z / band) v_y = velocity * TAN_TEST_ANGLE t_y = v_y / max_accel - d_y = .5 * v_y * t_y + d_y = 0.5 * v_y * t_y d_x = velocity * t_y notch_other_side = (notch_pos - d_x) * TAN_TEST_ANGLE + d_y - perimeter_offset = .5 * inner_size + perimeter_offset = 0.5 * inner_size for i in range(perimeters): # Move to the start of the perimeter next_y_offset = ( - perimeter_offset - notch_other_side - - (perimeter_offset - .5 * size) * TAN_TEST_ANGLE) - yield 'G1 X%.6f Y%.6f F%.3f' % ( - center_x - perimeter_offset, - center_y - next_y_offset, - velocity * 60.) - yield 'G1 Z%.6f' % (z,) + perimeter_offset + - notch_other_side + - (perimeter_offset - 0.5 * size) * TAN_TEST_ANGLE + ) + yield "G1 X%.6f Y%.6f F%.3f" % ( + center_x - perimeter_offset, + center_y - next_y_offset, + velocity * 60.0, + ) + yield "G1 Z%.6f" % (z,) # Print the perimiter loop for notch_axis in [(1, 0), (0, 1), (-1, 0), (0, -1)]: other_axis = (notch_axis[1], -notch_axis[0]) + def rotated_G1(x, y, e, v): - return 'G1 X%.6f Y%.6f E%.9f F%.3f' % ( - center_x + notch_axis[0] * x - + other_axis[0] * y, - center_y + notch_axis[1] * x - + other_axis[1] * y, - e * extr_r, v * 60.) - yield ('SET_VELOCITY_LIMIT ACCEL=%.3f ' - 'ACCEL_TO_DECEL=%.3f' % (max_accel, max_accel)) + return "G1 X%.6f Y%.6f E%.9f F%.3f" % ( + center_x + + notch_axis[0] * x + + other_axis[0] * y, + center_y + + notch_axis[1] * x + + other_axis[1] * y, + e * extr_r, + v * 60.0, + ) + + yield ( + "SET_VELOCITY_LIMIT ACCEL=%.3f " + "ACCEL_TO_DECEL=%.3f" % (max_accel, max_accel) + ) # The extrusion flow of the lines at an agle is reduced # by cos(angle) to maintain the correct spacing between # the perimeters formed by those lines yield rotated_G1( - notch_pos - d_x - 1.0 - .5 * size, - perimeter_offset - d_y - TAN_TEST_ANGLE, - (notch_pos - d_x - 1.0 - .5 * size - + perimeter_offset), - recipr_cos * velocity) - yield ('SET_VELOCITY_LIMIT ACCEL=%.6f' - + ' ACCEL_TO_DECEL=%.6f') % ( - INFINITE_ACCEL, INFINITE_ACCEL) + notch_pos - d_x - 1.0 - 0.5 * size, + perimeter_offset - d_y - TAN_TEST_ANGLE, + ( + notch_pos + - d_x + - 1.0 + - 0.5 * size + + perimeter_offset + ), + recipr_cos * velocity, + ) + yield ( + "SET_VELOCITY_LIMIT ACCEL=%.6f" + + " ACCEL_TO_DECEL=%.6f" + ) % (INFINITE_ACCEL, INFINITE_ACCEL) yield rotated_G1( - notch_pos - d_x - .5 * size, - perimeter_offset - d_y, - 1., recipr_cos * velocity) + notch_pos - d_x - 0.5 * size, + perimeter_offset - d_y, + 1.0, + recipr_cos * velocity, + ) old_x, old_y = d_x, d_y for j in range(deceleration_points): - x = ((deceleration_points - j - 1) * d_x - / deceleration_points) + x = ( + (deceleration_points - j - 1) + * d_x + / deceleration_points + ) t = (d_x - x) / velocity - y = d_y - t * (v_y - .5 * max_accel * t) - d = math.sqrt((x - old_x)**2 + (y - old_y)**2) + y = d_y - t * (v_y - 0.5 * max_accel * t) + d = math.sqrt((x - old_x) ** 2 + (y - old_y) ** 2) v = velocity * d / (old_x - x) yield rotated_G1( - notch_pos - x - .5 * size, - perimeter_offset - y, - (old_x - x), v) + notch_pos - x - 0.5 * size, + perimeter_offset - y, + (old_x - x), + v, + ) old_x, old_y = x, y - yield ('SET_VELOCITY_LIMIT ACCEL=%.6f' - + ' ACCEL_TO_DECEL=%.6f') % ( - max_accel, .5 * max_accel) - if i < perimeters - 1 or (abs(band_part - .5) >= - .5 * LETTER_BAND_PART): + yield ( + "SET_VELOCITY_LIMIT ACCEL=%.6f" + + " ACCEL_TO_DECEL=%.6f" + ) % (max_accel, 0.5 * max_accel) + if i < perimeters - 1 or ( + abs(band_part - 0.5) >= 0.5 * LETTER_BAND_PART + ): yield rotated_G1( - next_y_offset, perimeter_offset, - next_y_offset - notch_pos + .5 * size, - velocity) + next_y_offset, + perimeter_offset, + next_y_offset - notch_pos + 0.5 * size, + velocity, + ) continue # Emboss a letter print_serif = ( - max(math.fmod(next_z, band) / band - .5, - .5 - math.fmod(prev_z, band) / band) - >= .5 * LETTER_BAND_PART - and abs(band_part - .5) < .5 * LETTER_BAND_PART) - letter_width = abs(band_part - .5) * band + nozzle - if notch_axis[0] and band_part < .5: + max( + math.fmod(next_z, band) / band - 0.5, + 0.5 - math.fmod(prev_z, band) / band, + ) + >= 0.5 * LETTER_BAND_PART + and abs(band_part - 0.5) < 0.5 * LETTER_BAND_PART + ) + letter_width = abs(band_part - 0.5) * band + nozzle + if notch_axis[0] and band_part < 0.5: # Bottom of Y letter letter_width = nozzle elem_size = nozzle if print_serif: letter_width += nozzle elem_size += nozzle - letter_perimeter_offset = -.5 * nozzle + letter_perimeter_offset = -0.5 * nozzle yield rotated_G1( - letter_offset - letter_width * .5, - perimeter_offset, - (letter_offset - letter_width * .5 - - notch_pos + .5 * size), - velocity) + letter_offset - letter_width * 0.5, + perimeter_offset, + ( + letter_offset + - letter_width * 0.5 + - notch_pos + + 0.5 * size + ), + velocity, + ) yield rotated_G1( - letter_offset - letter_width * .5, - perimeter_offset - letter_perimeter_offset, - abs(letter_perimeter_offset), velocity) - if letter_width > 2. * elem_size: + letter_offset - letter_width * 0.5, + perimeter_offset - letter_perimeter_offset, + abs(letter_perimeter_offset), + velocity, + ) + if letter_width > 2.0 * elem_size: yield rotated_G1( - (letter_offset - letter_width * .5 - + elem_size), - perimeter_offset - letter_perimeter_offset, - elem_size, velocity) + ( + letter_offset + - letter_width * 0.5 + + elem_size + ), + perimeter_offset - letter_perimeter_offset, + elem_size, + velocity, + ) yield rotated_G1( - (letter_offset - letter_width * .5 - + elem_size), - perimeter_offset, - abs(letter_perimeter_offset), velocity) + ( + letter_offset + - letter_width * 0.5 + + elem_size + ), + perimeter_offset, + abs(letter_perimeter_offset), + velocity, + ) yield rotated_G1( - (letter_offset + letter_width * .5 - - elem_size), - perimeter_offset, - letter_width - 2. * elem_size, velocity) + ( + letter_offset + + letter_width * 0.5 + - elem_size + ), + perimeter_offset, + letter_width - 2.0 * elem_size, + velocity, + ) yield rotated_G1( - (letter_offset + letter_width * .5 - - elem_size), - perimeter_offset - letter_perimeter_offset, - abs(letter_perimeter_offset), velocity) + ( + letter_offset + + letter_width * 0.5 + - elem_size + ), + perimeter_offset - letter_perimeter_offset, + abs(letter_perimeter_offset), + velocity, + ) yield rotated_G1( - letter_offset + letter_width * .5, - perimeter_offset - letter_perimeter_offset, - elem_size, velocity) + letter_offset + letter_width * 0.5, + perimeter_offset - letter_perimeter_offset, + elem_size, + velocity, + ) else: yield rotated_G1( - letter_offset + letter_width * .5, - perimeter_offset - letter_perimeter_offset, - letter_width, velocity) + letter_offset + letter_width * 0.5, + perimeter_offset - letter_perimeter_offset, + letter_width, + velocity, + ) yield rotated_G1( - letter_offset + letter_width * .5, - perimeter_offset, - abs(letter_perimeter_offset), velocity) + letter_offset + letter_width * 0.5, + perimeter_offset, + abs(letter_perimeter_offset), + velocity, + ) yield rotated_G1( - next_y_offset, - perimeter_offset, - (next_y_offset - letter_offset - - letter_width * .5), - velocity) + next_y_offset, + perimeter_offset, + ( + next_y_offset + - letter_offset + - letter_width * 0.5 + ), + velocity, + ) perimeter_offset += line_width self.progress = z / height prev_z, z = z, next_z - yield ('SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.f' - + ' VELOCITY=%.3f') % ( - old_max_accel, old_max_accel_to_decel, - old_max_velocity) + yield ( + "SET_VELOCITY_LIMIT ACCEL=%.3f ACCEL_TO_DECEL=%.f" + + " VELOCITY=%.3f" + ) % (old_max_accel, old_max_accel_to_decel, old_max_velocity) - yield 'M83' - yield 'G90' - yield 'M220 S100' + yield "M83" + yield "G90" + yield "M220 S100" for line in gen_brim(): yield line for line in gen_tower(): yield line if final_gcode_id is not None: yield "UPDATE_DELAYED_GCODE ID='%s' DURATION=0.01" % ( - final_gcode_id,) + final_gcode_id, + ) self.progress = 1.0 + def load_config(config): return RingingTest(config) diff --git a/klippy/extras/sdcard_loop.py b/klippy/extras/sdcard_loop.py index 95be2e23c..47220523a 100644 --- a/klippy/extras/sdcard_loop.py +++ b/klippy/extras/sdcard_loop.py @@ -12,7 +12,7 @@ def __init__(self, config): printer = config.get_printer() self.sdcard = sdcard = printer.load_object(config, "virtual_sdcard") self.sdcard_gcode_provider = sdcard.get_virtual_sdcard_gcode_provider() - self.gcode = printer.lookup_object('gcode') + self.gcode = printer.lookup_object("gcode") self.gcode.register_command( "SDCARD_LOOP_BEGIN", self.cmd_SDCARD_LOOP_BEGIN, @@ -50,17 +50,22 @@ def cmd_SDCARD_LOOP_DESIST(self, gcmd): raise gcmd.error("Only permitted outside of a SD file.") def loop_begin(self, count): - if (not self.sdcard.is_cmd_from_sd() or - not self.sdcard_gcode_provider.is_active()): + if ( + not self.sdcard.is_cmd_from_sd() + or not self.sdcard_gcode_provider.is_active() + ): # Can only run inside of an SD file return False self.loop_stack.append( - (count, self.sdcard_gcode_provider.get_file_position())) + (count, self.sdcard_gcode_provider.get_file_position()) + ) return True def loop_end(self): - if (not self.sdcard.is_cmd_from_sd() or - not self.sdcard_gcode_provider.is_active()): + if ( + not self.sdcard.is_cmd_from_sd() + or not self.sdcard_gcode_provider.is_active() + ): # Can only run inside of an SD file return False # If the stack is empty, no need to skip back @@ -68,7 +73,7 @@ def loop_end(self): return True # Get iteration count and return position count, position = self.loop_stack.pop() - if count == 0: # Infinite loop + if count == 0: # Infinite loop self.sdcard_gcode_provider.set_file_position(position) self.loop_stack.append((0, position)) elif count == 1: # Last repeat @@ -82,8 +87,10 @@ def loop_end(self): return True def loop_desist(self): - if (self.sdcard.is_cmd_from_sd() and - self.sdcard_gcode_provider.is_active()): + if ( + self.sdcard.is_cmd_from_sd() + and self.sdcard_gcode_provider.is_active() + ): # Can only run outside of an SD file return False logging.info("Desisting existing SD loops") diff --git a/klippy/extras/shaper_calibrate.py b/klippy/extras/shaper_calibrate.py index 6a5767a32..8dcafde41 100644 --- a/klippy/extras/shaper_calibrate.py +++ b/klippy/extras/shaper_calibrate.py @@ -18,8 +18,17 @@ TEST_DAMPING_RATIOS = [0.075, 0.1, 0.15] -AUTOTUNE_SHAPERS = ['smooth_zv', 'smooth_mzv', 'smooth_ei', 'smooth_2hump_ei', - 'smooth_zvd_ei', 'smooth_si', 'mzv', 'ei', '2hump_ei'] +AUTOTUNE_SHAPERS = [ + "smooth_zv", + "smooth_mzv", + "smooth_ei", + "smooth_2hump_ei", + "smooth_zvd_ei", + "smooth_si", + "mzv", + "ei", + "2hump_ei", +] ###################################################################### # Frequency response calculation and shaper auto-tuning @@ -34,16 +43,23 @@ def __init__(self, freq_bins, psd_sum, psd_x, psd_y, psd_z): self.psd_y = psd_y self.psd_z = psd_z self._psd_list = [self.psd_sum, self.psd_x, self.psd_y, self.psd_z] - self._psd_map = {'x': self.psd_x, 'y': self.psd_y, 'z': self.psd_z, - 'all': self.psd_sum} + self._psd_map = { + "x": self.psd_x, + "y": self.psd_y, + "z": self.psd_z, + "all": self.psd_sum, + } + def add_data(self, other): np = self.numpy for psd, other_psd in zip(self._psd_list, other._psd_list): # `other` data may be defined at different frequency bins, # interpolating to fix that. other_normalized = np.interp( - self.freq_bins, other.freq_bins, other_psd) + self.freq_bins, other.freq_bins, other_psd + ) psd[:] = np.maximum(psd, other_normalized) + def set_numpy(self, numpy): self.numpy = numpy @@ -51,8 +67,11 @@ def normalize_to_frequencies(self): freq_bins = self.freq_bins for psd in self._psd_list: # Avoid division by zero errors and remove low-frequency noise - psd *= self.numpy.tanh(.5 / MIN_FREQ * freq_bins) / (freq_bins + .1) - def get_psd(self, axis='all'): + psd *= self.numpy.tanh(0.5 / MIN_FREQ * freq_bins) / ( + freq_bins + 0.1 + ) + + def get_psd(self, axis="all"): return self._psd_map[axis] @@ -63,83 +82,91 @@ def get_psd(self, axis='all'): def step_response(np, t, omega, damping_ratio): - t = np.maximum(t, 0.) + t = np.maximum(t, 0.0) omega = np.swapaxes(np.array(omega, ndmin=2), 0, 1) damping = damping_ratio * omega - omega_d = omega * math.sqrt(1. - damping_ratio**2) + omega_d = omega * math.sqrt(1.0 - damping_ratio**2) phase = math.acos(damping_ratio) - return (1. - np.exp((-damping * t)) - * np.sin((omega_d * t) + phase) * (1. / math.sin(phase))) + return 1.0 - np.exp((-damping * t)) * np.sin((omega_d * t) + phase) * ( + 1.0 / math.sin(phase) + ) + def step_response_min_velocity(damping_ratio): d2 = damping_ratio * damping_ratio - d_r = damping_ratio / math.sqrt(1. - d2) + d_r = damping_ratio / math.sqrt(1.0 - d2) # Analytical formula for the minimum was obtained using Maxima system - t = .5 * math.atan2(2. * d2, (2. * d2 - 1.) * d_r) + math.pi + t = 0.5 * math.atan2(2.0 * d2, (2.0 * d2 - 1.0) * d_r) + math.pi phase = math.acos(damping_ratio) v = math.exp(-d_r * t) * (d_r * math.sin(t + phase) - math.cos(t + phase)) return v + def estimate_shaper_old(np, shaper, test_damping_ratio, test_freqs): A, T = np.asarray(shaper[0]), np.asarray(shaper[1]) - inv_D = 1. / A.sum() + inv_D = 1.0 / A.sum() - omega = 2. * math.pi * np.asarray(test_freqs) + omega = 2.0 * math.pi * np.asarray(test_freqs) damping = test_damping_ratio * omega - omega_d = omega * math.sqrt(1. - test_damping_ratio**2) + omega_d = omega * math.sqrt(1.0 - test_damping_ratio**2) W = A * np.exp(np.outer(-damping, (T[-1] - T))) S = W * np.sin(np.outer(omega_d, T)) C = W * np.cos(np.outer(omega_d, T)) - return np.sqrt(S.sum(axis=1)**2 + C.sum(axis=1)**2) * inv_D + return np.sqrt(S.sum(axis=1) ** 2 + C.sum(axis=1) ** 2) * inv_D + def estimate_shaper(np, shaper, test_damping_ratio, test_freqs): A, T = np.asarray(shaper[0]), np.asarray(shaper[1]) - inv_D = 1. / A.sum() + inv_D = 1.0 / A.sum() n = len(T) t_s = T[-1] - T[0] test_freqs = np.asarray(test_freqs) t_start = T[0] - t_end = T[-1] + 2.0 * np.maximum(1. / test_freqs[test_freqs > 0.], t_s) + t_end = T[-1] + 2.0 * np.maximum(1.0 / test_freqs[test_freqs > 0.0], t_s) n_t = 1000 - unity_range = np.linspace(0., 1., n_t) + unity_range = np.linspace(0.0, 1.0, n_t) time = (t_end[:, np.newaxis] - t_start) * unity_range + t_start - dt = (time[:,-1] - time[:,0]) / n_t + dt = (time[:, -1] - time[:, 0]) / n_t min_v = -step_response_min_velocity(test_damping_ratio) - omega = 2. * math.pi * test_freqs[test_freqs > 0.] + omega = 2.0 * math.pi * test_freqs[test_freqs > 0.0] response = np.zeros(shape=(omega.shape[0], time.shape[-1])) for i in range(n): s_r = step_response(np, time - T[i], omega, test_damping_ratio) response += A[i] * s_r response *= inv_D - velocity = (response[:,1:] - response[:,:-1]) / (omega * dt)[:, np.newaxis] + velocity = (response[:, 1:] - response[:, :-1]) / (omega * dt)[ + :, np.newaxis + ] res = np.zeros(shape=test_freqs.shape) - res[test_freqs > 0.] = -velocity.min(axis=-1) / min_v - res[test_freqs <= 0.] = 1. + res[test_freqs > 0.0] = -velocity.min(axis=-1) / min_v + res[test_freqs <= 0.0] = 1.0 return res + def estimate_smoother_old(np, smoother, test_damping_ratio, test_freqs): C, t_sm = smoother[0], smoother[1] hst = t_sm * 0.5 test_freqs = np.asarray(test_freqs) - omega = 2. * math.pi * test_freqs + omega = 2.0 * math.pi * test_freqs damping = test_damping_ratio * omega - omega_d = omega * math.sqrt(1. - test_damping_ratio**2) + omega_d = omega * math.sqrt(1.0 - test_damping_ratio**2) n_t = max(100, 100 * round(t_sm * np.max(test_freqs))) - t, dt = np.linspace(0., t_sm, n_t, retstep=True) + t, dt = np.linspace(0.0, t_sm, n_t, retstep=True) w = np.zeros(shape=t.shape) for c in C[::-1]: - w = w * (t-hst) + c + w = w * (t - hst) + c + + E = w * np.exp(np.outer(damping, (t - t_sm))) + C = np.cos(np.outer(omega_d, (t - t_sm))) + S = np.sin(np.outer(omega_d, (t - t_sm))) + return np.sqrt(np.trapz(E * C, dx=dt) ** 2 + np.trapz(E * S, dx=dt) ** 2) - E = w * np.exp(np.outer(damping, (t-t_sm))) - C = np.cos(np.outer(omega_d, (t-t_sm))) - S = np.sin(np.outer(omega_d, (t-t_sm))) - return np.sqrt(np.trapz(E * C, dx=dt)**2 + np.trapz(E * S, dx=dt)**2) def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): C, t_sm = smoother[0], smoother[1] @@ -148,25 +175,25 @@ def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): test_freqs = np.asarray(test_freqs) t_start = -t_sm - t_end = hst + np.maximum(1.5 / test_freqs[test_freqs > 0.], 2.0 * t_sm) + t_end = hst + np.maximum(1.5 / test_freqs[test_freqs > 0.0], 2.0 * t_sm) n_t = 1000 - unity_range = np.linspace(0., 1., n_t) + unity_range = np.linspace(0.0, 1.0, n_t) time = (t_end[:, np.newaxis] - t_start) * unity_range + t_start - dt = (time[:,-1] - time[:,0]) / n_t + dt = (time[:, -1] - time[:, 0]) / n_t tau = np.copy(time) - tau[time > hst] = 0. - tau[time < -hst] = 0. + tau[time > hst] = 0.0 + tau[time < -hst] = 0.0 w = np.zeros(shape=tau.shape) for c in C[::-1]: w = w * tau + c - w[time > hst] = 0. - w[time < -hst] = 0. + w[time > hst] = 0.0 + w[time < -hst] = 0.0 norms = (w * dt[:, np.newaxis]).sum(axis=-1) min_v = -step_response_min_velocity(test_damping_ratio) - omega = 2. * math.pi * test_freqs[test_freqs > 0.] + omega = 2.0 * math.pi * test_freqs[test_freqs > 0.0] wm = np.count_nonzero(time < -hst, axis=-1).min() wp = np.count_nonzero(time <= hst, axis=-1).max() @@ -174,18 +201,22 @@ def estimate_smoother(np, smoother, test_damping_ratio, test_freqs): def get_windows(m, wl): nrows = m.shape[-1] - wl + 1 n = m.strides[-1] - return np.lib.stride_tricks.as_strided(m, shape=(m.shape[0], nrows, wl), - strides=(m.strides[0], n, n)) + return np.lib.stride_tricks.as_strided( + m, shape=(m.shape[0], nrows, wl), strides=(m.strides[0], n, n) + ) s_r = step_response(np, time, omega, test_damping_ratio) w_dt = w[:, wm:wp] * (np.reciprocal(norms) * dt)[:, np.newaxis] - response = np.einsum("ijk,ik->ij", get_windows(s_r, wp - wm), w_dt[:,::-1]) - velocity = (response[:,1:] - response[:,:-1]) / (omega * dt)[:, np.newaxis] + response = np.einsum("ijk,ik->ij", get_windows(s_r, wp - wm), w_dt[:, ::-1]) + velocity = (response[:, 1:] - response[:, :-1]) / (omega * dt)[ + :, np.newaxis + ] res = np.zeros(shape=test_freqs.shape) - res[test_freqs > 0.] = -velocity.min(axis=-1) / min_v - res[test_freqs <= 0.] = 1. + res[test_freqs > 0.0] = -velocity.min(axis=-1) / min_v + res[test_freqs <= 0.0] = 1.0 return res + class ShaperCalibrate: def __init__(self, printer): self.printer = printer @@ -321,10 +352,14 @@ def _estimate_remaining_vibrations(self, freq_bins, vals, psd): # Calculate the acceptable level of remaining vibrations. # Note that these are not true remaining vibrations, but rather # just a score to compare different shapers between each other. - vibr_threshold = ((psd[freq_bins > 0] / freq_bins[freq_bins > 0]).max() - * (freq_bins + MIN_FREQ) * (1. / 33.3)) - remaining_vibrations = (self.numpy.maximum( - vals * psd - vibr_threshold, 0) * freq_bins**2).sum() + vibr_threshold = ( + (psd[freq_bins > 0] / freq_bins[freq_bins > 0]).max() + * (freq_bins + MIN_FREQ) + * (1.0 / 33.3) + ) + remaining_vibrations = ( + self.numpy.maximum(vals * psd - vibr_threshold, 0) * freq_bins**2 + ).sum() all_vibrations = (psd * freq_bins**2).sum() return remaining_vibrations / all_vibrations @@ -337,21 +372,25 @@ def _get_shaper_smoothing(self, shaper, accel=5000, scv=5.0): ts = shaper_defs.get_shaper_offset(A, T) # Calculate offset for 90 and 180 degrees turn - offset_90_x = offset_90_y = offset_180 = 0. + offset_90_x = offset_90_y = offset_180 = 0.0 for i in range(n): if T[i] >= ts: # Calculate offset for one of the axes - offset_90_x += A[i] * (scv + half_accel * (T[i]-ts)) * (T[i]-ts) + offset_90_x += ( + A[i] * (scv + half_accel * (T[i] - ts)) * (T[i] - ts) + ) else: - offset_90_y += A[i] * (scv - half_accel * (T[i]-ts)) * (T[i]-ts) - offset_180 += A[i] * half_accel * (T[i]-ts)**2 + offset_90_y += ( + A[i] * (scv - half_accel * (T[i] - ts)) * (T[i] - ts) + ) + offset_180 += A[i] * half_accel * (T[i] - ts) ** 2 offset_90 = inv_D * math.sqrt(offset_90_x**2 + offset_90_y**2) offset_180 *= inv_D return max(offset_90, abs(offset_180)) - def _get_smoother_smoothing(self, smoother, accel=5000, scv=5.): + def _get_smoother_smoothing(self, smoother, accel=5000, scv=5.0): np = self.numpy - half_accel = accel * .5 + half_accel = accel * 0.5 C, t_sm = smoother hst = 0.5 * t_sm @@ -359,23 +398,29 @@ def _get_smoother_smoothing(self, smoother, accel=5000, scv=5.): w = np.zeros(shape=t.shape) for c in C[::-1]: w = w * (-t) + c - inv_norm = 1. / np.trapz(w, dx=dt) + inv_norm = 1.0 / np.trapz(w, dx=dt) w *= inv_norm t -= np.trapz(t * w, dx=dt) offset_180 = np.trapz(half_accel * t**2 * w, dx=dt) offset_90_x = np.trapz(((scv + half_accel * t) * t * w)[t >= 0], dx=dt) - offset_90_y = np.trapz(((scv - half_accel * t) * t * w)[t < 0], dx=dt) + offset_90_y = np.trapz(((scv - half_accel * t) * t * w)[t < 0], dx=dt) offset_90 = math.sqrt(offset_90_x**2 + offset_90_y**2) return max(offset_90, abs(offset_180)) - def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, - estimate_shaper, get_shaper_smoothing): + def fit_shaper( + self, + shaper_cfg, + calibration_data, + max_smoothing, + estimate_shaper, + get_shaper_smoothing, + ): np = self.numpy shaper = shaper_cfg.init_func(1.0, shaper_defs.DEFAULT_DAMPING_RATIO) - test_freq_bins = np.arange(0., 10., 0.01) + test_freq_bins = np.arange(0.0, 10.0, 0.01) test_shaper_vals = np.zeros(shape=test_freq_bins.shape) # Exact damping ratio of the printer is unknown, pessimizing # remaining vibrations over possible damping values @@ -383,7 +428,7 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, vals = estimate_shaper(self.numpy, shaper, dr, test_freq_bins) test_shaper_vals = np.maximum(test_shaper_vals, vals) - test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, .2) + test_freqs = np.arange(shaper_cfg.min_freq, MAX_SHAPER_FREQ, 0.2) freq_bins = calibration_data.freq_bins psd = calibration_data.psd_sum[freq_bins <= MAX_FREQ] @@ -393,21 +438,27 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, results = [] for test_freq in test_freqs[::-1]: shaper = shaper_cfg.init_func( - test_freq, shaper_defs.DEFAULT_DAMPING_RATIO) + test_freq, shaper_defs.DEFAULT_DAMPING_RATIO + ) shaper_smoothing = get_shaper_smoothing(shaper) if max_smoothing and shaper_smoothing > max_smoothing and best_res: return best_res - shaper_vals = np.interp(freq_bins, test_freq_bins * test_freq, - test_shaper_vals) + shaper_vals = np.interp( + freq_bins, test_freq_bins * test_freq, test_shaper_vals + ) shaper_vibrations = self._estimate_remaining_vibrations( - freq_bins, shaper_vals, psd) + freq_bins, shaper_vals, psd + ) max_accel = self.find_max_accel(shaper, get_shaper_smoothing) # The score trying to minimize vibrations, but also accounting # the growth of smoothing. The formula itself does not have any # special meaning, it simply shows good results on real user data - shaper_score = shaper_smoothing * (2. * shaper_vibrations**1.5 + - shaper_vibrations * .2 + .001 + - shaper_smoothing * .002) + shaper_score = shaper_smoothing * ( + 2.0 * shaper_vibrations**1.5 + + shaper_vibrations * 0.2 + + 0.001 + + shaper_smoothing * 0.002 + ) results.append( CalibrationResult( name=shaper_cfg.name, @@ -427,21 +478,22 @@ def fit_shaper(self, shaper_cfg, calibration_data, max_smoothing, selected = best_res for res in results[::-1]: if res.score < selected.score and ( - res.vibrs < best_res.vibrs * 1.2 or - res.vibrs < best_res.vibrs + 0.0075): + res.vibrs < best_res.vibrs * 1.2 + or res.vibrs < best_res.vibrs + 0.0075 + ): selected = res return selected - def _bisect(self, func, eps = 1e-8): - left = right = 1. + def _bisect(self, func, eps=1e-8): + left = right = 1.0 while not func(left): right = left left *= 0.5 if right == left: while func(right): - right *= 2. + right *= 2.0 while right - left > eps: - middle = (left + right) * .5 + middle = (left + right) * 0.5 if func(middle): left = middle else: @@ -452,8 +504,10 @@ def find_max_accel(self, s, get_smoothing): # Just some empirically chosen value which produces good projections # for max_accel without much smoothing TARGET_SMOOTHING = 0.12 - max_accel = self._bisect(lambda test_accel: get_smoothing( - s, test_accel) <= TARGET_SMOOTHING, 1e-2) + max_accel = self._bisect( + lambda test_accel: get_smoothing(s, test_accel) <= TARGET_SMOOTHING, + 1e-2, + ) return max_accel def find_best_shaper(self, calibration_data, max_smoothing, logger=None): @@ -462,45 +516,86 @@ def find_best_shaper(self, calibration_data, max_smoothing, logger=None): for smoother_cfg in shaper_defs.INPUT_SMOOTHERS: if smoother_cfg.name not in AUTOTUNE_SHAPERS: continue - smoother = self.background_process_exec(self.fit_shaper, ( - smoother_cfg, calibration_data, max_smoothing, - estimate_smoother, self._get_smoother_smoothing)) + smoother = self.background_process_exec( + self.fit_shaper, + ( + smoother_cfg, + calibration_data, + max_smoothing, + estimate_smoother, + self._get_smoother_smoothing, + ), + ) if logger is not None: - logger("Fitted smoother '%s' frequency = %.1f Hz " - "(vibration score = %.2f%%, smoothing ~= %.3f," - " combined score = %.3e)" % ( - smoother.name, smoother.freq, smoother.vibrs * 100., - smoother.smoothing, smoother.score)) - logger("To avoid too much smoothing with '%s', suggested " - "max_accel <= %.0f mm/sec^2" % ( - smoother.name, - round(smoother.max_accel / 100.) * 100.)) + logger( + "Fitted smoother '%s' frequency = %.1f Hz " + "(vibration score = %.2f%%, smoothing ~= %.3f," + " combined score = %.3e)" + % ( + smoother.name, + smoother.freq, + smoother.vibrs * 100.0, + smoother.smoothing, + smoother.score, + ) + ) + logger( + "To avoid too much smoothing with '%s', suggested " + "max_accel <= %.0f mm/sec^2" + % (smoother.name, round(smoother.max_accel / 100.0) * 100.0) + ) all_shapers.append(smoother) - if (best_shaper is None or smoother.score * 1.2 < best_shaper.score - or (smoother.score * 1.03 < best_shaper.score and - smoother.smoothing * 1.01 < best_shaper.smoothing)): + if ( + best_shaper is None + or smoother.score * 1.2 < best_shaper.score + or ( + smoother.score * 1.03 < best_shaper.score + and smoother.smoothing * 1.01 < best_shaper.smoothing + ) + ): # Either the smoother significantly improves the score (by 20%), # or it improves the score and smoothing (by 5% and 10% resp.) best_shaper = smoother for shaper_cfg in shaper_defs.INPUT_SHAPERS: if shaper_cfg.name not in AUTOTUNE_SHAPERS: continue - shaper = self.background_process_exec(self.fit_shaper, ( - shaper_cfg, calibration_data, max_smoothing, - estimate_shaper, self._get_shaper_smoothing)) + shaper = self.background_process_exec( + self.fit_shaper, + ( + shaper_cfg, + calibration_data, + max_smoothing, + estimate_shaper, + self._get_shaper_smoothing, + ), + ) if logger is not None: - logger("Fitted shaper '%s' frequency = %.1f Hz " - "(vibration score = %.2f%%, smoothing ~= %.3f," - " combined score = %.3e)" % ( - shaper.name, shaper.freq, shaper.vibrs * 100., - shaper.smoothing, shaper.score)) - logger("To avoid too much smoothing with '%s', suggested " - "max_accel <= %.0f mm/sec^2" % ( - shaper.name, round(shaper.max_accel / 100.) * 100.)) + logger( + "Fitted shaper '%s' frequency = %.1f Hz " + "(vibration score = %.2f%%, smoothing ~= %.3f," + " combined score = %.3e)" + % ( + shaper.name, + shaper.freq, + shaper.vibrs * 100.0, + shaper.smoothing, + shaper.score, + ) + ) + logger( + "To avoid too much smoothing with '%s', suggested " + "max_accel <= %.0f mm/sec^2" + % (shaper.name, round(shaper.max_accel / 100.0) * 100.0) + ) all_shapers.append(shaper) - if (best_shaper is None or shaper.score * 1.2 < best_shaper.score or - (shaper.score * 1.03 < best_shaper.score and - shaper.smoothing * 1.01 < best_shaper.smoothing)): + if ( + best_shaper is None + or shaper.score * 1.2 < best_shaper.score + or ( + shaper.score * 1.03 < best_shaper.score + and shaper.smoothing * 1.01 < best_shaper.smoothing + ) + ): # Either the shaper significantly improves the score (by 20%), # or it improves the score and smoothing (by 5% and 10% resp.) best_shaper = shaper diff --git a/klippy/extras/shaper_defs.py b/klippy/extras/shaper_defs.py index 9c26ac4cc..065faaa63 100644 --- a/klippy/extras/shaper_defs.py +++ b/klippy/extras/shaper_defs.py @@ -9,9 +9,12 @@ DEFAULT_DAMPING_RATIO = 0.1 InputShaperCfg = collections.namedtuple( - 'InputShaperCfg', ('name', 'init_func', 'min_freq')) + "InputShaperCfg", ("name", "init_func", "min_freq") +) InputSmootherCfg = collections.namedtuple( - 'InputSmootherCfg', ('name', 'init_func', 'min_freq')) + "InputSmootherCfg", ("name", "init_func", "min_freq") +) + def get_none_shaper(): return ([], []) @@ -101,24 +104,28 @@ def get_3hump_ei_shaper(shaper_freq, damping_ratio): T = [0.0, 0.5 * t_d, t_d, 1.5 * t_d, 2.0 * t_d] return (A, T) + def get_shaper_offset(A, T): if not A: - return 0. + return 0.0 return sum([a * t for a, t in zip(A, T)]) / sum(A) + def init_smoother(coeffs, smooth_time, normalize_coeffs): if not normalize_coeffs: return (list(reversed(coeffs)), smooth_time) - inv_t_sm = inv_t_sm_n = 1. / smooth_time + inv_t_sm = inv_t_sm_n = 1.0 / smooth_time n = len(coeffs) c = [0] * n - for i in range(n-1,-1,-1): - c[n-i-1] = coeffs[i] * inv_t_sm_n + for i in range(n - 1, -1, -1): + c[n - i - 1] = coeffs[i] * inv_t_sm_n inv_t_sm_n *= inv_t_sm return (c, smooth_time) + def get_none_smoother(): - return ([1.], 0.) + return ([1.0], 0.0) + # Input smoother is a polynomial function w(t) which is convolved with the input # trajectory x(t) as x_sm(T) = integral(x(t) * w(T-t) * dt, t=[T-T_sm...T+T_sm]) @@ -144,79 +151,172 @@ def get_none_smoother(): # input smoothers usable as extruder smoothers (for the ease of synchronization # of the extruder with the toolhead motion). -def get_zv_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-118.4265334338076,5.861885495127615,29.52796003014231, - -1.465471373781904,0.01966833207740377] + +def get_zv_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -118.4265334338076, + 5.861885495127615, + 29.52796003014231, + -1.465471373781904, + 0.01966833207740377, + ] return init_smoother(coeffs, 0.8025 / shaper_freq, normalize_coeffs) -def get_mzv_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-1906.717580206364,125.8892756660212,698.0200035767849, - -37.75923018121473,-62.18762409216703,1.57172781617736, - 1.713117990217123] + +def get_mzv_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -1906.717580206364, + 125.8892756660212, + 698.0200035767849, + -37.75923018121473, + -62.18762409216703, + 1.57172781617736, + 1.713117990217123, + ] return init_smoother(coeffs, 0.95625 / shaper_freq, normalize_coeffs) -def get_ei_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-1797.048868963208,120.5310596109878,669.6653197989012, - -35.71975707450795,-62.49388325512682,1.396748042940248, - 1.848276903900512] + +def get_ei_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -1797.048868963208, + 120.5310596109878, + 669.6653197989012, + -35.71975707450795, + -62.49388325512682, + 1.396748042940248, + 1.848276903900512, + ] return init_smoother(coeffs, 1.06625 / shaper_freq, normalize_coeffs) -def get_2hump_ei_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-22525.88434486782,2524.826047114184,10554.22832043971, - -1051.778387878068,-1475.914693073253,121.2177946817349, - 57.95603221424528,-4.018706414213658,0.8375784787864095] + +def get_2hump_ei_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -22525.88434486782, + 2524.826047114184, + 10554.22832043971, + -1051.778387878068, + -1475.914693073253, + 121.2177946817349, + 57.95603221424528, + -4.018706414213658, + 0.8375784787864095, + ] return init_smoother(coeffs, 1.14875 / shaper_freq, normalize_coeffs) -def get_si_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-6186.76006449789,1206.747198930197,2579.985143622855, - -476.8554763069169,-295.546608490564,52.69679971161049, - 4.234582468800491,-2.226157642004671,1.267781046297883] + +def get_si_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -6186.76006449789, + 1206.747198930197, + 2579.985143622855, + -476.8554763069169, + -295.546608490564, + 52.69679971161049, + 4.234582468800491, + -2.226157642004671, + 1.267781046297883, + ] return init_smoother(coeffs, 1.245 / shaper_freq, normalize_coeffs) -def get_zvd_ei_smoother(shaper_freq, damping_ratio_unused=None, - normalize_coeffs=True): - coeffs = [-18835.07746719777,1914.349309746547,8786.608981369287, - -807.3061869131075,-1209.429748155012,96.48879052981883, - 43.1595785340444,-3.577268915175282,1.083220648523371] + +def get_zvd_ei_smoother( + shaper_freq, damping_ratio_unused=None, normalize_coeffs=True +): + coeffs = [ + -18835.07746719777, + 1914.349309746547, + 8786.608981369287, + -807.3061869131075, + -1209.429748155012, + 96.48879052981883, + 43.1595785340444, + -3.577268915175282, + 1.083220648523371, + ] return init_smoother(coeffs, 1.475 / shaper_freq, normalize_coeffs) + def get_extr_zv_smoother(smooth_time, normalize_coeffs=True): - coeffs = [30., 0., -15., 0., 1.875] + coeffs = [30.0, 0.0, -15.0, 0.0, 1.875] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_mzv_shaper_smoother(smooth_time, normalize_coeffs=True): - coeffs = [341.76438367888272,-27.466545717280418,-153.088062685115716, - 13.7332728586402091,12.4632094027673599,-1.7166591073300261, - 1.1121330721453511] + coeffs = [ + 341.76438367888272, + -27.466545717280418, + -153.088062685115716, + 13.7332728586402091, + 12.4632094027673599, + -1.7166591073300261, + 1.1121330721453511, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_zvd_shaper_smoother(smooth_time, normalize_coeffs=True): - coeffs = [630.74844408176386,-22.308531752627160,-307.90095218665919, - 11.1542658763135805,35.685142827998881,-1.3942832345391976, - 0.4670793658889200] + coeffs = [ + 630.74844408176386, + -22.308531752627160, + -307.90095218665919, + 11.1542658763135805, + 35.685142827998881, + -1.3942832345391976, + 0.4670793658889200, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_ei_shaper_smoother(smooth_time, normalize_coeffs=True): - coeffs = [479.19339444799374,-30.811311356643330,-226.71074702571096, - 15.4056556783216649,23.506612053856646,-1.9257069597902081, - 0.8053718873928709] + coeffs = [ + 479.19339444799374, + -30.811311356643330, + -226.71074702571096, + 15.4056556783216649, + 23.506612053856646, + -1.9257069597902081, + 0.8053718873928709, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_2hump_ei_shaper_smoother(smooth_time, normalize_coeffs=True): - coeffs = [6732.1368755766744,-451.56419782599357,-3806.9806651540493, - 208.13010436901791,666.92541201595748,-19.3967650921351584, - -40.412386015136598,-1.1032496589986802,1.6069214755974468] + coeffs = [ + 6732.1368755766744, + -451.56419782599357, + -3806.9806651540493, + 208.13010436901791, + 666.92541201595748, + -19.3967650921351584, + -40.412386015136598, + -1.1032496589986802, + 1.6069214755974468, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_3hump_ei_shaper_smoother(smooth_time, normalize_coeffs=True): - coeffs = [4862.4802465588073,-540.91386960353532,-2666.8882179975822, - 256.61643727342101,445.67339875133428,-26.886868086047631, - -26.700173911045901,-0.8650310955216656,1.4965209988949661] + coeffs = [ + 4862.4802465588073, + -540.91386960353532, + -2666.8882179975822, + 256.61643727342101, + 445.67339875133428, + -26.886868086047631, + -26.700173911045901, + -0.8650310955216656, + 1.4965209988949661, + ] # Ideally, the following coefficients should be used, but this order # is currently not supported by C integration routines # coeffs = [101624.001181926069,-10140.6488068767758,-67034.280052740767, @@ -225,53 +325,96 @@ def get_extr_3hump_ei_shaper_smoother(smooth_time, normalize_coeffs=True): # -3.1226733743674644,1.0704518836439141] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_mzv_smoother(smooth_time, normalize_coeffs=True): - coeffs = [128.462077073626915,-42.165459650911941,-38.818969860871568, - 21.0827298254559707,-4.6771545208692649,-2.6353412281819963, - 1.5882542922463685] + coeffs = [ + 128.462077073626915, + -42.165459650911941, + -38.818969860871568, + 21.0827298254559707, + -4.6771545208692649, + -2.6353412281819963, + 1.5882542922463685, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_ei_smoother(smooth_time, normalize_coeffs=True): - coeffs = [276.191048111389079,-41.835391901208922,-117.95949005967274, - 20.9176959506044611,7.1939235089509097,-2.6147119938255576, - 1.2585021247513637] + coeffs = [ + 276.191048111389079, + -41.835391901208922, + -117.95949005967274, + 20.9176959506044611, + 7.1939235089509097, + -2.6147119938255576, + 1.2585021247513637, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_2hump_ei_smoother(smooth_time, normalize_coeffs=True): - coeffs = [1672.36152804372318,-70.342256841452126,-786.13315506927438, - -0.9529934764915708,102.73410996847843,13.665669896018062, - -8.4896839114829827,-2.2577576185761026,1.4522074338774609] + coeffs = [ + 1672.36152804372318, + -70.342256841452126, + -786.13315506927438, + -0.9529934764915708, + 102.73410996847843, + 13.665669896018062, + -8.4896839114829827, + -2.2577576185761026, + 1.4522074338774609, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_zvd_ei_smoother(smooth_time, normalize_coeffs=True): - coeffs = [1829.54723817218109,-746.70578009311191,-927.21920387422937, - 393.61004675467614,145.568898884847072,-56.797689609879597, - -13.2775511017668428,1.2660722942575129,1.5624627565635203] + coeffs = [ + 1829.54723817218109, + -746.70578009311191, + -927.21920387422937, + 393.61004675467614, + 145.568898884847072, + -56.797689609879597, + -13.2775511017668428, + 1.2660722942575129, + 1.5624627565635203, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_extr_si_smoother(smooth_time, normalize_coeffs=True): - coeffs = [114.962413010742423,164.830749583661599,90.845373435141965, - -132.210144372387873,-42.617667050825957,35.199306639257386, - -2.9098248068475399,-3.1121730987848171,1.5225295066412017] + coeffs = [ + 114.962413010742423, + 164.830749583661599, + 90.845373435141965, + -132.210144372387873, + -42.617667050825957, + 35.199306639257386, + -2.9098248068475399, + -3.1121730987848171, + 1.5225295066412017, + ] return init_smoother(coeffs, smooth_time, normalize_coeffs) + def get_smoother_offset(C, t_sm, normalized=True): - int_t0 = int_t1 = 0. + int_t0 = int_t1 = 0.0 if normalized: for i, c in enumerate(C): if i & 1: - int_t1 += c * t_sm**(i+2) / (2**(i+1) * (i+2)) + int_t1 += c * t_sm ** (i + 2) / (2 ** (i + 1) * (i + 2)) else: - int_t0 += c * t_sm**(i+1) / (2**i * (i+1)) + int_t0 += c * t_sm ** (i + 1) / (2**i * (i + 1)) else: for i, c in enumerate(C): if i & 1: - int_t1 += c / (2**(i+1) * (i+2)) + int_t1 += c / (2 ** (i + 1) * (i + 2)) else: - int_t0 += c / (2**i * (i+1)) + int_t0 += c / (2**i * (i + 1)) int_t1 *= t_sm return int_t1 / int_t0 + # min_freq for each shaper is chosen to have projected max_accel ~= 1500 INPUT_SHAPERS = [ InputShaperCfg("zv", get_zv_shaper, min_freq=21.0), @@ -284,24 +427,24 @@ def get_smoother_offset(C, t_sm, normalized=True): # min_freq for each smoother is chosen to have projected max_accel ~= 1000 INPUT_SMOOTHERS = [ - InputSmootherCfg('smooth_zv', get_zv_smoother, min_freq=18.), - InputSmootherCfg('smooth_mzv', get_mzv_smoother, min_freq=20.), - InputSmootherCfg('smooth_ei', get_ei_smoother, min_freq=21.), - InputSmootherCfg('smooth_2hump_ei', get_2hump_ei_smoother, min_freq=21.5), - InputSmootherCfg('smooth_zvd_ei', get_zvd_ei_smoother, min_freq=26.), - InputSmootherCfg('smooth_si', get_si_smoother, min_freq=21.5), + InputSmootherCfg("smooth_zv", get_zv_smoother, min_freq=18.0), + InputSmootherCfg("smooth_mzv", get_mzv_smoother, min_freq=20.0), + InputSmootherCfg("smooth_ei", get_ei_smoother, min_freq=21.0), + InputSmootherCfg("smooth_2hump_ei", get_2hump_ei_smoother, min_freq=21.5), + InputSmootherCfg("smooth_zvd_ei", get_zvd_ei_smoother, min_freq=26.0), + InputSmootherCfg("smooth_si", get_si_smoother, min_freq=21.5), ] -EXTURDER_SMOOTHERS = { - 'default' : get_extr_zv_smoother, - 'mzv' : get_extr_mzv_shaper_smoother, - 'zvd' : get_extr_zvd_shaper_smoother, - 'ei' : get_extr_ei_shaper_smoother, - '2hump_ei' : get_extr_2hump_ei_shaper_smoother, - '3hump_ei' : get_extr_3hump_ei_shaper_smoother, - 'smooth_mzv' : get_extr_mzv_smoother, - 'smooth_ei' : get_extr_ei_smoother, - 'smooth_2hump_ei' : get_extr_2hump_ei_smoother, - 'smooth_zvd_ei' : get_extr_zvd_ei_smoother, - 'smooth_si' : get_extr_si_smoother, +EXTRUDER_SMOOTHERS = { + "default": get_extr_zv_smoother, + "mzv": get_extr_mzv_shaper_smoother, + "zvd": get_extr_zvd_shaper_smoother, + "ei": get_extr_ei_shaper_smoother, + "2hump_ei": get_extr_2hump_ei_shaper_smoother, + "3hump_ei": get_extr_3hump_ei_shaper_smoother, + "smooth_mzv": get_extr_mzv_smoother, + "smooth_ei": get_extr_ei_smoother, + "smooth_2hump_ei": get_extr_2hump_ei_smoother, + "smooth_zvd_ei": get_extr_zvd_ei_smoother, + "smooth_si": get_extr_si_smoother, } diff --git a/klippy/extras/virtual_sdcard.py b/klippy/extras/virtual_sdcard.py index fc1a6fc8f..cc70589b2 100644 --- a/klippy/extras/virtual_sdcard.py +++ b/klippy/extras/virtual_sdcard.py @@ -15,16 +15,17 @@ def __init__(self, config): # sdcard state sd = config.get("path") self.sdcard_dirname = os.path.normpath(os.path.expanduser(sd)) - self.filename = '' + self.filename = "" self.current_file = None self.file_position = self.file_size = 0 self.next_file_position = 0 # Register commands - self.gcode = self.printer.lookup_object('gcode') - for cmd in ['M20', 'M21', 'M26', 'M27']: - self.gcode.register_command(cmd, getattr(self, 'cmd_' + cmd)) - for cmd in ['M28', 'M29', 'M30']: + self.gcode = self.printer.lookup_object("gcode") + for cmd in ["M20", "M21", "M26", "M27"]: + self.gcode.register_command(cmd, getattr(self, "cmd_" + cmd)) + for cmd in ["M28", "M29", "M30"]: self.gcode.register_command(cmd, self.cmd_error) + # Generic methods of GCode provider def handle_shutdown(self): if self.current_file is not None: @@ -36,28 +37,38 @@ def handle_shutdown(self): except: logging.exception("virtual_sdcard shutdown read") return - logging.info("Virtual sdcard (%d): %s\nUpcoming (%d): %s", - readpos, repr(data[:readcount]), - self.file_position, repr(data[readcount:])) + logging.info( + "Virtual sdcard (%d): %s\nUpcoming (%d): %s", + readpos, + repr(data[:readcount]), + self.file_position, + repr(data[readcount:]), + ) + def get_stats(self, eventtime): return True, "sd_pos=%d" % (self.file_position,) + def get_status(self, eventtime): return { - 'file_path': self.file_path(), - 'progress': self.progress(), - 'file_position': self.file_position, - 'file_size': self.file_size, + "file_path": self.file_path(), + "progress": self.progress(), + "file_position": self.file_position, + "file_size": self.file_size, } + def is_active(self): return self.current_file is not None + def get_name(self): return self.filename + def reset(self): if self.current_file is not None: self.current_file.close() self.current_file = None - self.filename = '' + self.filename = "" self.file_position = self.file_size = 0 + def get_gcode(self): logging.info("Starting SD card print (position %d)", self.file_position) try: @@ -82,7 +93,7 @@ def get_gcode(self): logging.info("Finished SD card print") self.gcode.respond_raw("Done printing file") break - lines = data.split('\n') + lines = data.split("\n") lines[0] = partial_input + lines[0] partial_input = lines.pop() lines.reverse() @@ -103,6 +114,7 @@ def get_gcode(self): lines = [] partial_input = "" logging.info("Exiting SD card print (position %d)", self.file_position) + # Virtual SD Card file management def get_file_list(self, check_subdirs=False): if check_subdirs: @@ -132,6 +144,7 @@ def get_file_list(self, check_subdirs=False): except: logging.exception("virtual_sdcard get_file_list") raise self.gcode.error("Unable to get file list") + def file_path(self): if self.current_file: return self.current_file.name @@ -141,7 +154,8 @@ def progress(self): if self.file_size: return float(self.file_position) / self.file_size else: - return 0. + return 0.0 + def load_file(self, gcmd, filename, check_subdirs=False): files = self.get_file_list(check_subdirs) flist = [f[0] for f in files] @@ -164,13 +178,17 @@ def load_file(self, gcmd, filename, check_subdirs=False): self.file_position = 0 self.file_size = fsize self.filename = filename + def get_file_position(self): return self.next_file_position + def set_file_position(self, pos): self.next_file_position = pos + # G-Code commands def cmd_error(self, gcmd): raise gcmd.error("SD write not supported") + def cmd_M20(self, gcmd): # List SD card files = self.get_file_list() @@ -178,30 +196,36 @@ def cmd_M20(self, gcmd): for fname, fsize in files: gcmd.respond_raw("%s %d" % (fname, fsize)) gcmd.respond_raw("End file list") + def cmd_M21(self, gcmd): # Initialize SD card gcmd.respond_raw("SD card ok") + def cmd_M26(self, gcmd): # Set SD position if not self.is_active(): gcmd.respond_raw("Not printing from SD card.") - pos = gcmd.get_int('S', minval=0) + pos = gcmd.get_int("S", minval=0) self.set_file_position(pos) + def cmd_M27(self, gcmd): # Report SD print status if not self.is_active(): gcmd.respond_raw("Not printing from SD card.") return - gcmd.respond_raw("SD printing byte %d/%d" - % (self.file_position, self.file_size)) + gcmd.respond_raw( + "SD printing byte %d/%d" % (self.file_position, self.file_size) + ) + class VirtualSD: def __init__(self, config): self.printer = config.get_printer() - self.printer.register_event_handler("klippy:shutdown", - self.handle_shutdown) + self.printer.register_event_handler( + "klippy:shutdown", self.handle_shutdown + ) # Print Stat Tracking - self.print_stats = self.printer.load_object(config, 'print_stats') + self.print_stats = self.printer.load_object(config, "print_stats") # sdcard state self.virtualsd_gcode_provider = VirtualSDGCodeProvider(config) self.gcode_provider = None @@ -210,67 +234,85 @@ def __init__(self, config): self.must_pause_work = self.cmd_from_sd = False self.work_timer = None # Error handling - gcode_macro = self.printer.load_object(config, 'gcode_macro') + gcode_macro = self.printer.load_object(config, "gcode_macro") self.on_error_gcode = gcode_macro.load_template( - config, 'on_error_gcode', '') + config, "on_error_gcode", "" + ) # Register commands - self.gcode = self.printer.lookup_object('gcode') - for cmd in ['M23', 'M24', 'M25']: - self.gcode.register_command(cmd, getattr(self, 'cmd_' + cmd)) + self.gcode = self.printer.lookup_object("gcode") + for cmd in ["M23", "M24", "M25"]: + self.gcode.register_command(cmd, getattr(self, "cmd_" + cmd)) self.gcode.register_command( - "SDCARD_RESET_FILE", self.cmd_SDCARD_RESET_FILE, - desc=self.cmd_SDCARD_RESET_FILE_help) + "SDCARD_RESET_FILE", + self.cmd_SDCARD_RESET_FILE, + desc=self.cmd_SDCARD_RESET_FILE_help, + ) self.gcode.register_command( - "SDCARD_PRINT_FILE", self.cmd_SDCARD_PRINT_FILE, - desc=self.cmd_SDCARD_PRINT_FILE_help) + "SDCARD_PRINT_FILE", + self.cmd_SDCARD_PRINT_FILE, + desc=self.cmd_SDCARD_PRINT_FILE_help, + ) + def handle_shutdown(self): if self.work_timer is not None: self.must_pause_work = True self.gcode_provider.handle_shutdown() + def get_file_list(self, check_subdirs=False): return self.virtualsd_gcode_provider.get_file_list(check_subdirs) + def stats(self, eventtime): if self.work_timer is None: return False, "" return self.gcode_provider.get_stats(eventtime) + def get_status(self, eventtime): - sts = {'is_active': self.is_active()} + sts = {"is_active": self.is_active()} if self.gcode_provider: sts.update(self.gcode_provider.get_status(eventtime)) else: sts.update(self.virtualsd_gcode_provider.get_status(eventtime)) return sts + def is_active(self): return self.work_timer is not None + def do_pause(self): if self.work_timer is not None: self.must_pause_work = True while self.work_timer is not None and not self.cmd_from_sd: - self.reactor.pause(self.reactor.monotonic() + .001) + self.reactor.pause(self.reactor.monotonic() + 0.001) + def do_resume(self): if self.work_timer is not None: raise self.gcode.error("SD busy") self.must_pause_work = False self.work_timer = self.reactor.register_timer( - self.work_handler, self.reactor.NOW) + self.work_handler, self.reactor.NOW + ) + def do_cancel(self): if self.gcode_provider is not None: self.do_pause() self.gcode_provider.reset() self.print_stats.note_cancel() self.gcode_provider = None + def _set_gcode_provider(self, gcode_provider): if self.gcode_provider is not None: raise self.gcode.error( - 'Print is active when resetting GCode provider') + "Print is active when resetting GCode provider" + ) self.gcode_provider = gcode_provider self.print_stats.set_current_file(gcode_provider.get_name()) self.gcode_lines = gcode_provider.get_gcode() - self.current_line = '' + self.current_line = "" + def print_with_gcode_provider(self, gcode_provider): self._reset_print() self._set_gcode_provider(gcode_provider) self.do_resume() + # G-Code commands def _reset_print(self): if self.gcode_provider is not None: @@ -279,37 +321,47 @@ def _reset_print(self): self.gcode_provider = None self.print_stats.reset() self.printer.send_event("virtual_sdcard:reset_file") - cmd_SDCARD_RESET_FILE_help = "Clears a loaded SD File. Stops the print "\ - "if necessary" + + cmd_SDCARD_RESET_FILE_help = ( + "Clears a loaded SD File. Stops the print " "if necessary" + ) + def cmd_SDCARD_RESET_FILE(self, gcmd): if self.cmd_from_sd: - raise gcmd.error( - "SDCARD_RESET_FILE cannot be run from the sdcard") + raise gcmd.error("SDCARD_RESET_FILE cannot be run from the sdcard") self._reset_print() - cmd_SDCARD_PRINT_FILE_help = "Loads a SD file and starts the print. May "\ + + cmd_SDCARD_PRINT_FILE_help = ( + "Loads a SD file and starts the print. May " "include files in subdirectories." + ) + def cmd_SDCARD_PRINT_FILE(self, gcmd): if self.work_timer is not None: raise gcmd.error("SD busy") self._reset_print() filename = gcmd.get("FILENAME") - if filename[0] == '/': + if filename[0] == "/": filename = filename[1:] - self.virtualsd_gcode_provider.load_file(gcmd, filename, - check_subdirs=True) + self.virtualsd_gcode_provider.load_file( + gcmd, filename, check_subdirs=True + ) self._set_gcode_provider(self.virtualsd_gcode_provider) self.do_resume() + def cmd_M23(self, gcmd): # Select SD file if self.work_timer is not None: raise gcmd.error("SD busy") self._reset_print() filename = gcmd.get_raw_command_parameters().strip() - if filename.startswith('/'): + if filename.startswith("/"): filename = filename[1:] - self.virtualsd_gcode_provider.load_file(gcmd, filename, - check_subdirs=True) + self.virtualsd_gcode_provider.load_file( + gcmd, filename, check_subdirs=True + ) self._set_gcode_provider(self.virtualsd_gcode_provider) + def cmd_M24(self, gcmd): # Start/resume SD print self.do_resume() @@ -317,10 +369,13 @@ def cmd_M24(self, gcmd): def cmd_M25(self, gcmd): # Pause SD print self.do_pause() + def get_virtual_sdcard_gcode_provider(self): return self.virtualsd_gcode_provider + def get_gcode_provider(self): return self.gcode_provider + def is_cmd_from_sd(self): return self.cmd_from_sd @@ -364,7 +419,7 @@ def work_handler(self, eventtime): logging.exception("virtual_sdcard dispatch") break self.cmd_from_sd = False - self.current_line = '' + self.current_line = "" self.work_timer = None self.cmd_from_sd = False if error_message is not None: diff --git a/klippy/kinematics/extruder.py b/klippy/kinematics/extruder.py index ef6bd3be4..5785dbfd6 100644 --- a/klippy/kinematics/extruder.py +++ b/klippy/kinematics/extruder.py @@ -6,141 +6,199 @@ import math, logging import stepper, chelper + class ExtruderSmoother: def __init__(self, config, pa_model): self.smooth_time = config.getfloat( - 'pressure_advance_smooth_time', 0.040, above=0., maxval=.200) + "pressure_advance_smooth_time", 0.040, above=0.0, maxval=0.200 + ) # A 4-th order smoothing function that goes to 0 together with # its derivative at the ends of the smoothing interval - self.a = [15./8., 0., -15., 0., 30.] + self.a = [15.0 / 8.0, 0.0, -15.0, 0.0, 30.0] self.pa_model = pa_model - self.axes = ['x', 'y', 'z'] + self.axes = ["x", "y", "z"] + def update(self, gcmd): - self.smooth_time = gcmd.get_float('SMOOTH_TIME', self.smooth_time) + self.smooth_time = gcmd.get_float("SMOOTH_TIME", self.smooth_time) + def update_pa_model(self, pa_model): self.pa_model = pa_model + def enable_axis(self, axis): if axis not in self.axes: self.axes.append(axis) + def disable_axis(self, axis): if axis in self.axes: self.axes.remove(axis) + def update_extruder_kinematics(self, extruder_sk): ffi_main, ffi_lib = chelper.get_ffi() n = len(self.a) success = True - smooth_time = self.smooth_time if self.pa_model.enabled() else 0. + smooth_time = self.smooth_time if self.pa_model.enabled() else 0.0 for axis in self.axes: - if not ffi_lib.extruder_set_smoothing_params( - extruder_sk, axis.encode(), n, self.a, - smooth_time, 0.) == 0: + if ( + not ffi_lib.extruder_set_smoothing_params( + extruder_sk, axis.encode(), n, self.a, smooth_time, 0.0 + ) + == 0 + ): success = False return success + def get_status(self, eventtime): - return {'smooth_time': self.smooth_time} + return {"smooth_time": self.smooth_time} + def get_msg(self): return "pressure_advance_smooth_time: %.6f" % (self.smooth_time,) + class PALinearModel: - name = 'linear' + name = "linear" + def __init__(self, config=None): if config: self.pressure_advance = config.getfloat( - 'pressure_advance', 0., minval=0.) + "pressure_advance", 0.0, minval=0.0 + ) else: - self.pressure_advance = 0. + self.pressure_advance = 0.0 + def update(self, gcmd): self.pressure_advance = gcmd.get_float( - 'ADVANCE', self.pressure_advance, minval=0.) + "ADVANCE", self.pressure_advance, minval=0.0 + ) + def enabled(self): - return self.pressure_advance > 0. + return self.pressure_advance > 0.0 + def get_pa_params(self): return (self.pressure_advance,) + def get_status(self, eventtime): - return {'pressure_advance': self.pressure_advance} + return {"pressure_advance": self.pressure_advance} + def get_msg(self): - return 'pressure_advance: %.6f' % (self.pressure_advance,) + return "pressure_advance: %.6f" % (self.pressure_advance,) + def get_func(self): ffi_main, ffi_lib = chelper.get_ffi() return ffi_lib.pressure_advance_linear_model_func + class PANonLinearModel: def __init__(self, config=None): if config: - self.linear_advance = config.getfloat('linear_advance', - 0., minval=0.) - self.linear_offset = config.getfloat('linear_offset', - 0., minval=0.) + self.linear_advance = config.getfloat( + "linear_advance", 0.0, minval=0.0 + ) + self.linear_offset = config.getfloat( + "linear_offset", 0.0, minval=0.0 + ) if self.linear_offset: self.linearization_velocity = config.getfloat( - 'linearization_velocity', above=0.) + "linearization_velocity", above=0.0 + ) else: self.linearization_velocity = config.getfloat( - 'linearization_velocity', 0., minval=0.) + "linearization_velocity", 0.0, minval=0.0 + ) else: - self.linear_advance = 0. - self.linear_offset = 0. - self.linearization_velocity = 0. + self.linear_advance = 0.0 + self.linear_offset = 0.0 + self.linearization_velocity = 0.0 + def update(self, gcmd): self.linear_advance = gcmd.get_float( - 'ADVANCE', self.linear_advance, minval=0.) + "ADVANCE", self.linear_advance, minval=0.0 + ) self.linear_offset = gcmd.get_float( - 'OFFSET', self.linear_offset, minval=0.) + "OFFSET", self.linear_offset, minval=0.0 + ) self.linearization_velocity = gcmd.get_float( - 'VELOCITY', self.linearization_velocity) - if self.linear_offset and self.linearization_velocity <= 0.: - raise gcmd.error('VELOCITY must be set to a positive value ' - 'when OFFSET is non-zero') + "VELOCITY", self.linearization_velocity + ) + if self.linear_offset and self.linearization_velocity <= 0.0: + raise gcmd.error( + "VELOCITY must be set to a positive value " + "when OFFSET is non-zero" + ) + def enabled(self): - return self.linear_advance > 0. or self.linear_offset > 0. + return self.linear_advance > 0.0 or self.linear_offset > 0.0 + def get_pa_params(self): # The order must match the order of parameters in the # pressure_advance_params struct in kin_extruder.c - return (self.linear_advance, self.linear_offset, - self.linearization_velocity) + return ( + self.linear_advance, + self.linear_offset, + self.linearization_velocity, + ) + def get_status(self, eventtime): - return {'linear_advance': self.linear_advance, - 'linear_offset': self.linear_offset, - 'linearization_velocity': self.linearization_velocity} + return { + "linear_advance": self.linear_advance, + "linear_offset": self.linear_offset, + "linearization_velocity": self.linearization_velocity, + } + def get_msg(self): - return ('linear_advance: %.6f\n' - 'linear_offset: %.6f\n' - 'linearization_velocity: %.6f' % ( - self.linear_advance, - self.linear_offset, - self.linearization_velocity)) + return ( + "linear_advance: %.6f\n" + "linear_offset: %.6f\n" + "linearization_velocity: %.6f" + % ( + self.linear_advance, + self.linear_offset, + self.linearization_velocity, + ) + ) + def get_func(self): return None + class PATanhModel(PANonLinearModel): - name = 'tanh' + name = "tanh" + def __init__(self, config=None): PANonLinearModel.__init__(self, config) + def get_func(self): ffi_main, ffi_lib = chelper.get_ffi() return ffi_lib.pressure_advance_tanh_model_func + class PAReciprModel(PANonLinearModel): - name = 'recipr' + name = "recipr" + def __init__(self, config=None): PANonLinearModel.__init__(self, config) + def get_func(self): ffi_main, ffi_lib = chelper.get_ffi() return ffi_lib.pressure_advance_recipr_model_func + class ExtruderStepper: - pa_models = {PALinearModel.name: PALinearModel, - PATanhModel.name: PATanhModel, - PAReciprModel.name: PAReciprModel} + pa_models = { + PALinearModel.name: PALinearModel, + PATanhModel.name: PATanhModel, + PAReciprModel.name: PAReciprModel, + } + def __init__(self, config): self.printer = config.get_printer() self.name = config.get_name().split()[-1] - self.pa_model = config.getchoice('pressure_advance_model', - self.pa_models, - PALinearModel.name)(config) + self.pa_model = config.getchoice( + "pressure_advance_model", self.pa_models, PALinearModel.name + )(config) self.smoother = ExtruderSmoother(config, self.pa_model) self.pressure_advance_time_offset = config.getfloat( - 'pressure_advance_time_offset', 0.0, minval=-0.2, maxval=0.2) + "pressure_advance_time_offset", 0.0, minval=-0.2, maxval=0.2 + ) # Setup stepper self.stepper = stepper.PrinterStepper(config) ffi_main, ffi_lib = chelper.get_ffi() @@ -149,7 +207,8 @@ def __init__(self, config): ) self.stepper.set_stepper_kinematics(self.sk_extruder) ffi_lib.extruder_set_pressure_advance_model_func( - self.sk_extruder, self.pa_model.get_func()) + self.sk_extruder, self.pa_model.get_func() + ) self.motion_queue = None self.extruder = None # Register commands @@ -202,24 +261,29 @@ def __init__(self, config): ) def _handle_connect(self): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.register_step_generator(self.stepper.generate_steps) - self._update_pressure_advance(self.pa_model, - self.pressure_advance_time_offset) + self._update_pressure_advance( + self.pa_model, self.pressure_advance_time_offset + ) self.smoother.update_extruder_kinematics(self.sk_extruder) + def get_status(self, eventtime): - sts = {'pressure_advance_model': self.pa_model.name, - 'time_offset': self.pressure_advance_time_offset, - 'motion_queue': self.motion_queue} + sts = { + "pressure_advance_model": self.pa_model.name, + "time_offset": self.pressure_advance_time_offset, + "motion_queue": self.motion_queue, + } sts.update(self.pa_model.get_status(eventtime)) sts.update(self.smoother.get_status(eventtime)) return sts + def find_past_position(self, print_time): mcu_pos = self.stepper.get_past_mcu_position(print_time) return self.stepper.mcu_to_commanded_position(mcu_pos) def sync_to_extruder(self, extruder_name): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() if not extruder_name: if self.extruder is not None: @@ -229,23 +293,27 @@ def sync_to_extruder(self, extruder_name): return extruder = self.printer.lookup_object(extruder_name, None) if extruder is None or not isinstance(extruder, PrinterExtruder): - raise self.printer.command_error("'%s' is not a valid extruder." - % (extruder_name,)) + raise self.printer.command_error( + "'%s' is not a valid extruder." % (extruder_name,) + ) extruder.link_extruder_stepper(self) self.motion_queue = extruder_name self.extruder = extruder + def _update_pressure_advance(self, pa_model, time_offset): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) if self.pa_model.name != pa_model.name: pa_func = pa_model.get_func() - ffi_lib.extruder_set_pressure_advance_model_func(self.sk_extruder, - pa_func) + ffi_lib.extruder_set_pressure_advance_model_func( + self.sk_extruder, pa_func + ) pa_params = pa_model.get_pa_params() ffi_lib.extruder_set_pressure_advance( - self.sk_extruder, len(pa_params), pa_params, time_offset) + self.sk_extruder, len(pa_params), pa_params, time_offset + ) self.smoother.update_pa_model(pa_model) self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) @@ -253,13 +321,15 @@ def _update_pressure_advance(self, pa_model, time_offset): toolhead.note_step_generation_scan_time(new_delay, old_delay) self.pa_model = pa_model self.pressure_advance_time_offset = time_offset + def update_input_shaping(self, shapers, exact_mode): ffi_main, ffi_lib = chelper.get_ffi() old_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) failed_shapers = [] for shaper in shapers: - if not shaper.update_extruder_kinematics(self.sk_extruder, - exact_mode): + if not shaper.update_extruder_kinematics( + self.sk_extruder, exact_mode + ): failed_shapers.append(shaper) # Pressure advance requires extruder smoothing, make sure that # some smoothing is enabled @@ -269,14 +339,15 @@ def update_input_shaping(self, shapers, exact_mode): self.smoother.enable_axis(shaper.get_axis()) self.smoother.update_extruder_kinematics(self.sk_extruder) new_delay = ffi_lib.extruder_get_step_gen_window(self.sk_extruder) - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") if old_delay != new_delay: toolhead.note_step_generation_scan_time(new_delay, old_delay) return failed_shapers + cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters" def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): - extruder = self.printer.lookup_object('toolhead').get_extruder() + extruder = self.printer.lookup_object("toolhead").get_extruder() extruder_steppers = extruder.get_extruder_steppers() if not extruder_steppers: raise gcmd.error("Active extruder does not have a stepper") @@ -285,8 +356,9 @@ def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd): if strapq is not extruder.get_trapq(): raise gcmd.error("Unable to infer active extruder stepper") extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd) + def cmd_SET_PRESSURE_ADVANCE(self, gcmd): - pa_model_name = gcmd.get('MODEL', self.pa_model.name) + pa_model_name = gcmd.get("MODEL", self.pa_model.name) if pa_model_name not in self.pa_models: raise gcmd.error("Invalid MODEL='%s' choice" % (pa_model_name,)) pa_model = self.pa_model @@ -294,15 +366,20 @@ def cmd_SET_PRESSURE_ADVANCE(self, gcmd): pa_model = self.pa_models[pa_model_name]() pa_model.update(gcmd) self.smoother.update(gcmd) - time_offset = gcmd.get_float('TIME_OFFSET', - self.pressure_advance_time_offset, - minval=-0.2, maxval=0.2) + time_offset = gcmd.get_float( + "TIME_OFFSET", + self.pressure_advance_time_offset, + minval=-0.2, + maxval=0.2, + ) self._update_pressure_advance(pa_model, time_offset) - msg = ("pressure_advance_model: %s\n" % (pa_model.name,) + - pa_model.get_msg() + "\n" + - self.smoother.get_msg() + - "\npressure_advance_time_offset: %.6f" - % (time_offset,)) + msg = ( + "pressure_advance_model: %s\n" % (pa_model.name,) + + pa_model.get_msg() + + "\n" + + self.smoother.get_msg() + + "\npressure_advance_time_offset: %.6f" % (time_offset,) + ) self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg)) gcmd.respond_info(msg, log=False) @@ -371,7 +448,7 @@ class PrinterExtruder: def __init__(self, config, extruder_num): self.printer = config.get_printer() self.name = config.get_name() - self.last_position = [0., 0., 0.] + self.last_position = [0.0, 0.0, 0.0] # Setup hotend heater shared_heater = config.get("shared_heater", None) pheaters = self.printer.load_object(config, "heaters") @@ -419,9 +496,11 @@ def __init__(self, config, extruder_num): self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves # Setup extruder stepper self.extruder_steppers = [] - if (config.get('step_pin', None) is not None - or config.get('dir_pin', None) is not None - or config.get('rotation_distance', None) is not None): + if ( + config.get("step_pin", None) is not None + or config.get("dir_pin", None) is not None + or config.get("rotation_distance", None) is not None + ): self.link_extruder_stepper(ExtruderStepper(config)) # Register commands gcode = self.printer.lookup_object("gcode") @@ -429,26 +508,34 @@ def __init__(self, config, extruder_num): toolhead.set_extruder(self, 0.0) gcode.register_command("M104", self.cmd_M104) gcode.register_command("M109", self.cmd_M109) - gcode.register_mux_command("ACTIVATE_EXTRUDER", "EXTRUDER", - self.name, self.cmd_ACTIVATE_EXTRUDER, - desc=self.cmd_ACTIVATE_EXTRUDER_help) + gcode.register_mux_command( + "ACTIVATE_EXTRUDER", + "EXTRUDER", + self.name, + self.cmd_ACTIVATE_EXTRUDER, + desc=self.cmd_ACTIVATE_EXTRUDER_help, + ) + def link_extruder_stepper(self, extruder_stepper): if extruder_stepper not in self.extruder_steppers: self.extruder_steppers.append(extruder_stepper) extruder_stepper.stepper.set_position(self.last_position) extruder_stepper.stepper.set_trapq(self.trapq) + def unlink_extruder_stepper(self, extruder_stepper): if extruder_stepper in self.extruder_steppers: self.extruder_steppers.remove(extruder_stepper) extruder_stepper.stepper.set_trapq(None) + def get_extruder_steppers(self): return self.extruder_steppers + def update_move_time(self, flush_time): self.trapq_finalize_moves(self.trapq, flush_time) def get_status(self, eventtime): sts = self.heater.get_status(eventtime) - sts['can_extrude'] = self.heater.can_extrude + sts["can_extrude"] = self.heater.can_extrude if self.extruder_steppers: sts.update(self.extruder_steppers[0].get_status(eventtime)) return sts @@ -521,19 +608,32 @@ def move(self, print_time, move): extr_r = [math.copysign(r * r, axis_r) for r in move.axes_r[:3]] else: # Extrude-only move, do not apply pressure advance - extr_r = [0., 0., axis_r] - self.trapq_append(self.trapq, print_time, - move.accel_t, move.cruise_t, move.decel_t, - extr_pos[0], extr_pos[1], extr_pos[2], - extr_r[0], extr_r[1], extr_r[2], - start_v, cruise_v, accel) + extr_r = [0.0, 0.0, axis_r] + self.trapq_append( + self.trapq, + print_time, + move.accel_t, + move.cruise_t, + move.decel_t, + extr_pos[0], + extr_pos[1], + extr_pos[2], + extr_r[0], + extr_r[1], + extr_r[2], + start_v, + cruise_v, + accel, + ) extr_d = abs(move.axes_d[3]) for i in range(3): self.last_position[i] += extr_d * extr_r[i] + def find_past_position(self, print_time): if not self.extruder_steppers: - return 0. + return 0.0 return self.extruder_steppers[0].find_past_position(print_time) + def cmd_M104(self, gcmd, wait=False): # Set Extruder Temperature temp = gcmd.get_float("S", 0.0) @@ -588,8 +688,10 @@ def calc_junction(self, prev_move, move): def get_name(self): return "" + def get_extruder_steppers(self): return [] + def get_heater(self): raise self.printer.command_error("Extruder not configured") diff --git a/klippy/stepper.py b/klippy/stepper.py index a2f263e9d..49b0685d0 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -3,7 +3,8 @@ # Copyright (C) 2016-2021 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. -import math, logging, collections +import collections +import math import chelper, msgproto @@ -20,9 +21,17 @@ class error(Exception): # Interface to low-level mcu and chelper code class MCU_stepper: - def __init__(self, name, high_precision_stepcompr, step_pin_params, - dir_pin_params, rotation_dist, steps_per_rotation, - step_pulse_duration=None, units_in_radians=False): + def __init__( + self, + name, + high_precision_stepcompr, + step_pin_params, + dir_pin_params, + rotation_dist, + steps_per_rotation, + step_pulse_duration=None, + units_in_radians=False, + ): self._name = name self._high_precision_stepcompr = high_precision_stepcompr self._rotation_dist = rotation_dist @@ -47,11 +56,13 @@ def __init__(self, name, high_precision_stepcompr, step_pin_params, self._active_callbacks = [] ffi_main, ffi_lib = chelper.get_ffi() if high_precision_stepcompr: - self._stepqueue = ffi_main.gc(ffi_lib.stepcompress_hp_alloc(oid), - ffi_lib.stepcompress_free) + self._stepqueue = ffi_main.gc( + ffi_lib.stepcompress_hp_alloc(oid), ffi_lib.stepcompress_free + ) else: - self._stepqueue = ffi_main.gc(ffi_lib.stepcompress_alloc(oid), - ffi_lib.stepcompress_free) + self._stepqueue = ffi_main.gc( + ffi_lib.stepcompress_alloc(oid), ffi_lib.stepcompress_free + ) ffi_lib.stepcompress_set_invert_sdir(self._stepqueue, self._invert_dir) self._mcu.register_stepqueue(self._stepqueue) self._stepper_kinematics = None @@ -111,23 +122,35 @@ def _build_config(self): step_pulse_ticks = self._mcu.seconds_to_clock(self._step_pulse_duration) self._mcu.add_config_cmd( "config_stepper oid=%d step_pin=%s dir_pin=%s invert_step=%d" - " step_pulse_ticks=%u" % (self._oid, self._step_pin, self._dir_pin, - invert_step, step_pulse_ticks)) - self._mcu.add_config_cmd("reset_step_clock oid=%d clock=0" - % (self._oid,), on_restart=True) + " step_pulse_ticks=%u" + % ( + self._oid, + self._step_pin, + self._dir_pin, + invert_step, + step_pulse_ticks, + ) + ) + self._mcu.add_config_cmd( + "reset_step_clock oid=%d clock=0" % (self._oid,), on_restart=True + ) if self._high_precision_stepcompr: try: step_cmd_tag = self._mcu.lookup_command( "queue_step_hp oid=%c interval=%u count=%hu " - "add=%hi add2=%hi shift=%hi").get_command_tag() + "add=%hi add2=%hi shift=%hi" + ).get_command_tag() except msgproto.MessageParser.error as e: cerror = self._mcu.get_printer().config_error - raise cerror(str(e) + ", maybe MCU firmware was compiled " + - "without high precision stepping support?") + raise cerror( + str(e) + + ", maybe MCU firmware was compiled " + + "without high precision stepping support?" + ) else: step_cmd_tag = self._mcu.lookup_command( - "queue_step oid=%c interval=%u count=%hu " - "add=%hi").get_command_tag() + "queue_step oid=%c interval=%u count=%hu " "add=%hi" + ).get_command_tag() dir_cmd_tag = self._mcu.lookup_command( "set_next_step_dir oid=%c dir=%c" ).get_command_tag() @@ -311,14 +334,24 @@ def PrinterStepper(config, units_in_radians=False): dir_pin = config.get("dir_pin") dir_pin_params = ppins.lookup_pin(dir_pin, can_invert=True) rotation_dist, steps_per_rotation = parse_step_distance( - config, units_in_radians, True) - step_pulse_duration = config.getfloat('step_pulse_duration', None, - minval=0., maxval=.001) - high_precision_stepcompr = config.getboolean('high_precision_step_compress', - False) - mcu_stepper = MCU_stepper(name, high_precision_stepcompr, step_pin_params, - dir_pin_params, rotation_dist, steps_per_rotation, - step_pulse_duration, units_in_radians) + config, units_in_radians, True + ) + step_pulse_duration = config.getfloat( + "step_pulse_duration", None, minval=0.0, maxval=0.001 + ) + high_precision_stepcompr = config.getboolean( + "high_precision_step_compress", False + ) + mcu_stepper = MCU_stepper( + name, + high_precision_stepcompr, + step_pin_params, + dir_pin_params, + rotation_dist, + steps_per_rotation, + step_pulse_duration, + units_in_radians, + ) # Register with helper modules for mname in ["stepper_enable", "force_move", "motion_report"]: m = printer.load_object(config, mname) diff --git a/scripts/calibrate_shaper.py b/scripts/calibrate_shaper.py index b386c5333..05a56dea1 100755 --- a/scripts/calibrate_shaper.py +++ b/scripts/calibrate_shaper.py @@ -110,22 +110,30 @@ def plot_freq_response( best_shaper_vals = None for shaper in shapers: label = "%s (%.1f Hz, vibr=%.1f%%, sm~=%.2f, accel<=%.f)" % ( - shaper.name.upper(), shaper.freq, - shaper.vibrs * 100., shaper.smoothing, - round(shaper.max_accel / 100.) * 100.) - linestyle = 'dotted' if shaper.name.startswith('smooth') else 'dashed' + shaper.name.upper(), + shaper.freq, + shaper.vibrs * 100.0, + shaper.smoothing, + round(shaper.max_accel / 100.0) * 100.0, + ) + linestyle = "dotted" if shaper.name.startswith("smooth") else "dashed" linewidth = 1.0 if shaper.name == selected_shaper: - linestyle = 'dashdot' + linestyle = "dashdot" linewidth = 2.0 best_shaper_vals = shaper.vals - ax2.plot(freqs, shaper.vals, label=label, linestyle=linestyle, - linewidth=linewidth) + ax2.plot( + freqs, + shaper.vals, + label=label, + linestyle=linestyle, + linewidth=linewidth, + ) vibr_thresh = (psd[freqs > 0] / freqs[freqs > 0]).max() * (freqs + 5) / 33.3 - ax.plot(freqs, vibr_thresh, - label='Acceptable\nvibrations', color='lightgrey') - ax.plot(freqs, psd * best_shaper_vals, - label='After\nshaper', color='cyan') + ax.plot( + freqs, vibr_thresh, label="Acceptable\nvibrations", color="lightgrey" + ) + ax.plot(freqs, psd * best_shaper_vals, label="After\nshaper", color="cyan") # A hack to add a human-readable shaper recommendation to legend ax2.plot( [], [], " ", label="Recommended shaper: %s" % (selected_shaper.upper()) diff --git a/scripts/graph_shaper.py b/scripts/graph_shaper.py index a0664ac2b..f2d945bf0 100755 --- a/scripts/graph_shaper.py +++ b/scripts/graph_shaper.py @@ -7,13 +7,15 @@ # This file may be distributed under the terms of the GNU GPLv3 license. import optparse, importlib, math, os, sys import numpy as np, matplotlib -sys.path.append(os.path.join(os.path.dirname(os.path.realpath(__file__)), - '..', 'klippy')) -shaper_defs = importlib.import_module('.shaper_defs', 'extras') -shaper_calibrate = importlib.import_module('.shaper_calibrate', 'extras') + +sys.path.append( + os.path.join(os.path.dirname(os.path.realpath(__file__)), "..", "klippy") +) +shaper_defs = importlib.import_module(".shaper_defs", "extras") +shaper_calibrate = importlib.import_module(".shaper_calibrate", "extras") # A set of damping ratios to calculate shaper response for -DAMPING_RATIOS=[0.075, 0.1, 0.15] +DAMPING_RATIOS = [0.075, 0.1, 0.15] # Parameters of the input shaper SHAPER_FREQ = 50.0 @@ -21,14 +23,14 @@ # Simulate input shaping of step function for these true resonance frequency # and damping ratio -STEP_SIMULATION_RESONANCE_FREQ=50. -STEP_SIMULATION_DAMPING_RATIO=0.1 +STEP_SIMULATION_RESONANCE_FREQ = 50.0 +STEP_SIMULATION_DAMPING_RATIO = 0.1 # If set, defines which range of frequencies to plot shaper frequency response PLOT_FREQ_RANGE = [] # If empty, will be automatically determined # PLOT_FREQ_RANGE = [10., 100.] -PLOT_FREQ_STEP = .05 +PLOT_FREQ_STEP = 0.05 ###################################################################### # Plotting and startup @@ -50,6 +52,7 @@ def bisect(func, left, right): def find_shaper_plot_range(estimate_shaper, shaper_freq, vib_tol): def eval_shaper(freq): return estimate_shaper(freq) - vib_tol + if not PLOT_FREQ_RANGE: left = bisect(eval_shaper, 0.1 * shaper_freq, shaper_freq) right = bisect(eval_shaper, shaper_freq, 3 * shaper_freq) @@ -57,22 +60,24 @@ def eval_shaper(freq): left, right = PLOT_FREQ_RANGE return (left, right) + def gen_shaper_response(shaper, shaper_freq, estimate_shaper): # Calculate shaper vibration response on a range of requencies freq_start, freq_end = find_shaper_plot_range( - lambda freq: estimate_shaper( - np, shaper, DAMPING_RATIOS[0], [freq]), - shaper_freq, vib_tol=0.25) + lambda freq: estimate_shaper(np, shaper, DAMPING_RATIOS[0], [freq]), + shaper_freq, + vib_tol=0.25, + ) freqs = np.arange(freq_start, freq_end, PLOT_FREQ_STEP) response = np.zeros(shape=(freqs.shape[0], len(DAMPING_RATIOS))) n_dr = len(DAMPING_RATIOS) response = np.zeros(shape=(freqs.shape[0], n_dr)) for i in range(n_dr): - response[:, i] = estimate_shaper( - np, shaper, DAMPING_RATIOS[i], freqs) - legend = ['damping ratio = %.3f' % d_r for d_r in DAMPING_RATIOS] + response[:, i] = estimate_shaper(np, shaper, DAMPING_RATIOS[i], freqs) + legend = ["damping ratio = %.3f" % d_r for d_r in DAMPING_RATIOS] return freqs, response, legend + def gen_shaped_step_function(shaper, freq, damping_ratio): # Calculate shaping of a step function A, T, _ = shaper @@ -81,33 +86,43 @@ def gen_shaped_step_function(shaper, freq, damping_ratio): t_s = T[-1] - T[0] A = np.asarray(A) - omega = 2. * math.pi * freq + omega = 2.0 * math.pi * freq - t_start = T[0] - .5 * t_s - t_end = T[-1] + 1.5 * max(1. / freq, t_s) - dt = .01 * min(t_s, 1. / freq) + t_start = T[0] - 0.5 * t_s + t_end = T[-1] + 1.5 * max(1.0 / freq, t_s) + dt = 0.01 * min(t_s, 1.0 / freq) time = np.arange(t_start, t_end, dt) time_t = time[np.newaxis].T step = np.zeros(time.shape) - step[time >= 0.] = 1. - commanded = np.sum(A * np.where(time_t - T >= 0, 1., 0.), axis=-1) * inv_D + step[time >= 0.0] = 1.0 + commanded = np.sum(A * np.where(time_t - T >= 0, 1.0, 0.0), axis=-1) * inv_D response = np.zeros(shape=(time.shape)) for i in range(n): - response += A[i] * shaper_calibrate.step_response( - np, time - T[i], omega, damping_ratio)[0,:] + response += ( + A[i] + * shaper_calibrate.step_response( + np, time - T[i], omega, damping_ratio + )[0, :] + ) response *= inv_D velocity = np.insert( - (response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.) - legend = ['step', 'shaper commanded', 'system response', - 'system response velocity'] + (response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.0 + ) + legend = [ + "step", + "shaper commanded", + "system response", + "system response velocity", + ] result = np.zeros(shape=(time.shape[0], 4)) - result[:,0] = step - result[:,1] = commanded - result[:,2] = response - result[:,3] = velocity + result[:, 0] = step + result[:, 1] = commanded + result[:, 2] = response + result[:, 3] = velocity return time, result, legend + def gen_smoothed_step_function(smoother, freq, damping_ratio): # Calculate smoothing of a step function C, t_sm, t_offs, _ = smoother @@ -119,36 +134,44 @@ def gen_smoothed_step_function(smoother, freq, damping_ratio): for c in C[::-1]: w = w * tau + c - omega = 2. * math.pi * freq + omega = 2.0 * math.pi * freq damping = damping_ratio * omega - omega_d = omega * math.sqrt(1. - damping_ratio**2) + omega_d = omega * math.sqrt(1.0 - damping_ratio**2) phase = math.acos(damping_ratio) t_start = -t_sm - t_offs - t_end = hst - t_offs + 1.5 * max(1. / freq, t_sm) + t_end = hst - t_offs + 1.5 * max(1.0 / freq, t_sm) time = np.arange(t_start, t_end, dt) time_t = time[np.newaxis].T w_dt = w * dt step = np.zeros(time.shape) - step[time >= 0.] = 1. - commanded = np.sum(w_dt * np.where(time_t - tau + t_offs >= 0, 1., 0.), - axis=-1) + step[time >= 0.0] = 1.0 + commanded = np.sum( + w_dt * np.where(time_t - tau + t_offs >= 0, 1.0, 0.0), axis=-1 + ) s_r_n = time.size + tau.size - 1 s_r_t = np.arange(t_start - hst, t_end + hst * 1.1, dt)[:s_r_n] s_r = shaper_calibrate.step_response( - np, s_r_t + t_offs, omega, damping_ratio) - response = np.convolve(s_r[0,:], w_dt, mode='valid') - velocity = np.insert((response[1:] - response[:-1]) - / (2 * math.pi * freq * dt), 0, 0.) - legend = ['step', 'shaper commanded', 'system response', - 'system response velocity'] + np, s_r_t + t_offs, omega, damping_ratio + ) + response = np.convolve(s_r[0, :], w_dt, mode="valid") + velocity = np.insert( + (response[1:] - response[:-1]) / (2 * math.pi * freq * dt), 0, 0.0 + ) + legend = [ + "step", + "shaper commanded", + "system response", + "system response velocity", + ] result = np.zeros(shape=(time.shape[0], 4)) - result[:,0] = step - result[:,1] = commanded - result[:,2] = response - result[:,3] = velocity + result[:, 0] = step + result[:, 1] = commanded + result[:, 2] = response + result[:, 3] = velocity return time, result, legend + def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): for s in shaper_defs.INPUT_SHAPERS: if s.name == shaper_name.lower(): @@ -157,23 +180,29 @@ def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): T = [t - ts for t in T] shaper = A, T, s.name.upper() freqs, response, response_legend = gen_shaper_response( - shaper, shaper_freq, shaper_calibrate.estimate_shaper) + shaper, shaper_freq, shaper_calibrate.estimate_shaper + ) time, step_vals, step_legend = gen_shaped_step_function( - shaper, freq, damping_ratio) + shaper, freq, damping_ratio + ) for s in shaper_defs.INPUT_SMOOTHERS: if s.name == shaper_name.lower(): C, t_sm = s.init_func(shaper_freq) t_offs = shaper_defs.get_smoother_offset(C, t_sm) shaper = C, t_sm, t_offs, s.name.upper() freqs, response, response_legend = gen_shaper_response( - shaper, shaper_freq, shaper_calibrate.estimate_smoother) + shaper, shaper_freq, shaper_calibrate.estimate_smoother + ) time, step_vals, step_legend = gen_smoothed_step_function( - shaper, freq, damping_ratio) + shaper, freq, damping_ratio + ) - fig, (ax1, ax2) = matplotlib.pyplot.subplots(nrows=2, figsize=(10,9)) - ax1.set_title("Vibration response simulation for shaper '%s',\n" - "shaper_freq=%.1f Hz, damping_ratio=%.3f" - % (shaper[-1], shaper_freq, SHAPER_DAMPING_RATIO)) + fig, (ax1, ax2) = matplotlib.pyplot.subplots(nrows=2, figsize=(10, 9)) + ax1.set_title( + "Vibration response simulation for shaper '%s',\n" + "shaper_freq=%.1f Hz, damping_ratio=%.3f" + % (shaper[-1], shaper_freq, SHAPER_DAMPING_RATIO) + ) ax1.plot(freqs, response) ax1.set_ylim(bottom=0.0) fontP = matplotlib.font_manager.FontProperties() @@ -186,8 +215,10 @@ def plot_shaper(shaper_name, shaper_freq, freq, damping_ratio): ax1.grid(which="major", color="grey") ax1.grid(which="minor", color="lightgrey") - ax2.set_title("Unit step input, resonance frequency=%.1f Hz, " - "damping ratio=%.3f" % (freq, damping_ratio)) + ax2.set_title( + "Unit step input, resonance frequency=%.1f Hz, " + "damping ratio=%.3f" % (freq, damping_ratio) + ) ax2.plot(time, step_vals) ax2.legend(step_legend, loc="best", prop=fontP) ax2.set_xlabel("Time, sec") @@ -209,32 +240,59 @@ def main(): # Parse command-line arguments usage = "%prog [options]" opts = optparse.OptionParser(usage) - opts.add_option("-o", "--output", type="string", dest="output", - default=None, help="filename of output graph") - opts.add_option("-s", "--shaper", type="string", dest="shaper", - default="ei", help="name of the shaper to plot") - opts.add_option("-f", "--freq", type="float", dest="freq", - default=STEP_SIMULATION_RESONANCE_FREQ, - help="the frequency of the system") - opts.add_option("--damping_ratio", type="float", dest="damping_ratio", - default=STEP_SIMULATION_DAMPING_RATIO, - help="the damping ratio of the system") - opts.add_option("--shaper_freq", type="float", dest="shaper_freq", - default=SHAPER_FREQ, - help="the frequency of the shaper") + opts.add_option( + "-o", + "--output", + type="string", + dest="output", + default=None, + help="filename of output graph", + ) + opts.add_option( + "-s", + "--shaper", + type="string", + dest="shaper", + default="ei", + help="name of the shaper to plot", + ) + opts.add_option( + "-f", + "--freq", + type="float", + dest="freq", + default=STEP_SIMULATION_RESONANCE_FREQ, + help="the frequency of the system", + ) + opts.add_option( + "--damping_ratio", + type="float", + dest="damping_ratio", + default=STEP_SIMULATION_DAMPING_RATIO, + help="the damping ratio of the system", + ) + opts.add_option( + "--shaper_freq", + type="float", + dest="shaper_freq", + default=SHAPER_FREQ, + help="the frequency of the shaper", + ) options, args = opts.parse_args() if len(args) != 0: opts.error("Incorrect number of arguments") - if (options.shaper.lower() not in - [s.name for s in shaper_defs.INPUT_SHAPERS] and - options.shaper.lower() not in - [s.name for s in shaper_defs.INPUT_SMOOTHERS]): + if options.shaper.lower() not in [ + s.name for s in shaper_defs.INPUT_SHAPERS + ] and options.shaper.lower() not in [ + s.name for s in shaper_defs.INPUT_SMOOTHERS + ]: opts.error("Invalid shaper name '%s'" % (opts.shaper,)) # Draw graph setup_matplotlib(options.output is not None) - fig = plot_shaper(options.shaper, options.shaper_freq, options.freq, - options.damping_ratio) + fig = plot_shaper( + options.shaper, options.shaper_freq, options.freq, options.damping_ratio + ) # Show graph if options.output is None: diff --git a/scripts/motan/readlog.py b/scripts/motan/readlog.py index 4196de131..f159ef557 100644 --- a/scripts/motan/readlog.py +++ b/scripts/motan/readlog.py @@ -15,11 +15,13 @@ class error(Exception): # Log data handlers ###################################################################### + def shift_interval(interval, shift): if shift <= 0: return interval << -shift else: - return (interval + (1 << (shift-1))) >> shift + return (interval + (1 << (shift - 1))) >> shift + # Log data handlers: {name: class, ...} LogHandlers = {} @@ -244,21 +246,23 @@ def _pull_block(self, req_time): if req_time <= last_time: break # Process block into (time, half_position, position) 3-tuples - first_time = step_time = jmsg['first_step_time'] - first_clock = jmsg['first_clock'] - step_clock = first_clock - shift_interval(jmsg['data'][0][0], - jmsg['data'][0][4]) - cdiff = jmsg['last_clock'] - first_clock + first_time = step_time = jmsg["first_step_time"] + first_clock = jmsg["first_clock"] + step_clock = first_clock - shift_interval( + jmsg["data"][0][0], jmsg["data"][0][4] + ) + cdiff = jmsg["last_clock"] - first_clock tdiff = last_time - first_time inv_freq = 0.0 if cdiff: inv_freq = tdiff / cdiff - step_dist = jmsg['step_distance'] - step_pos = jmsg['start_position'] - (step_dist if jmsg['data'][0][1] > 0 - else -step_dist) + step_dist = jmsg["step_distance"] + step_pos = jmsg["start_position"] - ( + step_dist if jmsg["data"][0][1] > 0 else -step_dist + ) if not step_data[0][0]: - step_data[0] = (0., step_pos, step_pos) - for interval, raw_count, add, add2, shift in jmsg['data']: + step_data[0] = (0.0, step_pos, step_pos) + for interval, raw_count, add, add2, shift in jmsg["data"]: qs_dist = step_dist count = raw_count if count < 0: @@ -360,20 +364,22 @@ def _pull_block(self, req_time): if req_time <= last_time: break # Process block into (time, position) 2-tuples - first_time = step_time = jmsg['first_step_time'] - first_clock = jmsg['first_clock'] - step_clock = first_clock - shift_interval(jmsg['data'][0][0], - jmsg['data'][0][4]) - cdiff = jmsg['last_clock'] - first_clock + first_time = step_time = jmsg["first_step_time"] + first_clock = jmsg["first_clock"] + step_clock = first_clock - shift_interval( + jmsg["data"][0][0], jmsg["data"][0][4] + ) + cdiff = jmsg["last_clock"] - first_clock tdiff = last_time - first_time inv_freq = 0.0 if cdiff: inv_freq = tdiff / cdiff - step_pos = jmsg['start_mcu_position'] - (1 if jmsg['data'][0][1] > 0 - else -1) + step_pos = jmsg["start_mcu_position"] - ( + 1 if jmsg["data"][0][1] > 0 else -1 + ) if not step_data[0][0]: - step_data[0] = (0., step_pos) - for interval, raw_count, add, add2, shift in jmsg['data']: + step_data[0] = (0.0, step_pos) + for interval, raw_count, add, add2, shift in jmsg["data"]: qs_dist = 1 count = raw_count if count < 0: