diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index e83e0de2e..16b138ad9 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -338,6 +338,28 @@ position_max: #use_sensorless_homing: # If true, disables the second home action if homing_retract_dist > 0. # The default is true if endstop_pin is configured to use virtual_endstop +#homing_samples: 1 +#homing_sample_retract_dist: +# The distance (in mm) to back the toolhead off between each sample (if +# sampling more than once). min_home_dist + homing_elapsed_distance_tolerance. +# Needs to be greater than min_home_dist for everything to work properly. +#homing_samples_result: average +#homing_samples_tolerance: 0.100 +#homing_samples_tolerance_retries: 0 +#homing_drop_first_result: False +# See the "probe" section for a description of the above parameters +# If the second homing is enabled, the first homing move will only be once and the +# second homing move will use samples. +#homing_sample_retract_speed: +# Equals probes lift_speed, defaults to retract_speed +#homing_move_toolhead_after_adjusting: False +# Move the toolhead to the configured homing position after adjusting the coordinate +# system according to the samples. +#homing_retry_gcode: +# For some kinematics, like CoreXY, if one axis skips while homing it also throws off +# the other axis so it would be good to rehome that axis. +# If the homing procedure has to retry at least once, this code will be executed after +# homing is completed. ``` ### Cartesian Kinematics diff --git a/docs/G-Codes.md b/docs/G-Codes.md index 317d05241..7d0993913 100644 --- a/docs/G-Codes.md +++ b/docs/G-Codes.md @@ -1407,6 +1407,16 @@ not recommended: "triggered" or in an "open" state. This command is typically used to verify that an endstop is working correctly. +### [homing_accuracy] + +#### HOMING_ACCURACY +`HOMING_ACCURACY AXIS= [DROP_FIRST_RESULT=] [SPEED=] [SAMPLES=] +[RETRACT_DIST=] [RETRACT_SPEED=]`: Calculate the maximum, minimum, average, +median, and standard deviation of multiple homing samples. By default, +10 SAMPLES are taken. Otherwise the optional parameters default to +their equivalent setting in the stepper config section. +Speed defaults to `second_homing_speed`. + ### [resonance_tester] The following commands are available when a diff --git a/klippy/extras/homing.py b/klippy/extras/homing.py index 233ad2f42..648032ab8 100644 --- a/klippy/extras/homing.py +++ b/klippy/extras/homing.py @@ -305,97 +305,256 @@ def _reset_endstop_states(self, endstops): for endstop in endstops: endstop[0].query_endstop(print_time) + def _calc_mean(self, distances): + return sum(distances) / float(len(distances)) + + def _calc_median(self, distances): + z_sorted = sorted(distances) + middle = len(distances) // 2 + if (len(distances) & 1) == 1: + # odd number of samples + return z_sorted[middle] + # even number of samples + return self._calc_mean(z_sorted[middle - 1 : middle + 1]) + + def init_homing(self, hi, homing_axes): + pass + + def process_homing_info(self, hi): + return hi + def home_rails(self, rails, forcepos, movepos): # Notify of upcoming homing operation self.printer.send_event("homing:home_rails_begin", self, rails) - # Alter kinematics class to think printer is at forcepo + gcode = self.printer.lookup_object("gcode") + # Alter kinematics class to think printer is at forcepos homing_axes = [axis for axis in range(3) if forcepos[axis] is not None] - startpos = self._fill_coord(forcepos) - homepos = self._fill_coord(movepos) - self.toolhead.set_position(startpos, homing_axes=homing_axes) # Perform first home endstops = [es for rail in rails for es in rail.get_endstops()] - hi = rails[0].get_homing_info() + hi = self.process_homing_info(rails[0].get_homing_info()) + self.init_homing(hi, homing_axes) + needs_rehome = False + retract_dist = hi.retract_dist + sample_retract_dist = hi.sample_retract_dist hmove = HomingMove(self.printer, endstops) - try: - self._set_homing_accel(hi.accel, pre_homing=True) - self._set_homing_current(homing_axes, pre_homing=True) - self._reset_endstop_states(endstops) - hmove.homing_move(homepos, hi.speed) - finally: - self._set_homing_accel(hi.accel, pre_homing=False) + retries = 0 + distances = [] + drop_result = hi.drop_first_result - needs_rehome = False - retract_dist = hi.retract_dist - if hmove.moved_less_than_dist(hi.min_home_dist, homing_axes): - needs_rehome = True - retract_dist = hi.min_home_dist - - # Perform second home - if retract_dist: - logging.info("homing: needs rehome: %s", needs_rehome) - # Retract + startpos = None + homepos = None + axes_d = [] + move_d = None + retract_r = None + retractpos = [] + + def _retract_toolhead(retract_distance, retract_speed): + nonlocal startpos, homepos, axes_d, move_d, retract_r, retractpos startpos = self._fill_coord(forcepos) homepos = self._fill_coord(movepos) axes_d = [hp - sp for hp, sp in zip(homepos, startpos)] move_d = math.sqrt(sum([d * d for d in axes_d[:3]])) - retract_r = min(1.0, retract_dist / move_d) + retract_r = min(1.0, retract_distance / move_d) retractpos = [ hp - ad * retract_r for hp, ad in zip(homepos, axes_d) ] - self.toolhead.move(retractpos, hi.retract_speed) - if not hi.use_sensorless_homing or needs_rehome: - try: - # Home again - startpos = [ - rp - ad * retract_r - for rp, ad in zip(retractpos, axes_d) - ] - self.toolhead.set_position(startpos) - self._reset_endstop_states(endstops) - - hmove = HomingMove(self.printer, endstops) - hmove.homing_move(homepos, hi.second_homing_speed) - - if hmove.check_no_movement() is not None: - raise self.printer.command_error( - "Endstop %s still triggered after retract" - % (hmove.check_no_movement(),) - ) - if ( - hi.use_sensorless_homing - and needs_rehome - and hmove.moved_less_than_dist( - hi.min_home_dist, homing_axes + self.toolhead.move(retractpos, retract_speed) + + # Process the homing sample + def _process_sample(trigpos): + nonlocal drop_result, distances, retries + # early return if we don't use samples for homing + if hi.sample_count == 1: + distances.append([0.0] * len(hmove.distance_elapsed)) + return + + # early return if we want to drop it + if drop_result: + # Don't process the sample if it's dropped + gcode.respond_info("Settling sample (ignored)...") + drop_result = False + else: + # since we need an initial reference, set the first sample as 0 + if not distances: + result = [0] * len(hmove.distance_elapsed) + else: + haltpos = self.toolhead.get_position() + result = [ + # Last deviation from the first home which is defined as 0.0 + distances[-1][i] + # deviation between retract and actual distance traveled till endstop triggered + + ( + (dist - sample_retract_dist) + if hi.positive_dir + else (dist + sample_retract_dist) ) + # compensate for the deviation between haltpos and trigpos + - (haltpos[i] - trigpos[i]) + if i in homing_axes + else 0 + for i, dist in enumerate(hmove.distance_elapsed) + ] + for i in homing_axes: + gcode.respond_info( + f"Homing sample for {'XYZ'[i]}: {result[i]:.9f}".rstrip( + "0" + ).rstrip(".") + ) + distances.append(result) + + if hi.samples_tolerance is not None: + # check if tolerance exceeds configured threshold + if any( + max([dist[i] for dist in distances]) + - min([dist[i] for dist in distances]) + > hi.samples_tolerance + for i in range(len(hmove.distance_elapsed)) ): - raise self.printer.command_error( - "Early homing trigger on second home!" + if retries >= hi.samples_retries: + raise self.printer.command_error( + "Homing samples exceed samples_tolerance" + ) + gcode.respond_info( + "Homing samples exceed tolerance. Retrying..." ) + retries += 1 + distances = [] + # retract unless we have all samples + if len(distances) < hi.sample_count: + _retract_toolhead( + hi.sample_retract_dist, hi.sample_retract_speed + ) + + try: + # home for each sample + while len(distances) < hi.sample_count: + startpos = self._fill_coord(forcepos) + homepos = self._fill_coord(movepos) + self.toolhead.set_position(startpos, homing_axes=homing_axes) + hmove = HomingMove(self.printer, endstops) + + try: + self._set_homing_accel(hi.accel, pre_homing=True) + self._set_homing_current(homing_axes, pre_homing=True) + self._reset_endstop_states(endstops) + trigpos = hmove.homing_move(homepos, hi.speed) finally: self._set_homing_accel(hi.accel, pre_homing=False) - self._set_homing_current(homing_axes, pre_homing=False) - - if hi.retract_dist: - # Retract (again) - startpos = self._fill_coord(forcepos) - homepos = self._fill_coord(movepos) - axes_d = [hp - sp for hp, sp in zip(homepos, startpos)] - move_d = math.sqrt(sum([d * d for d in axes_d[:3]])) - retract_r = min(1.0, hi.retract_dist / move_d) - retractpos = [ - hp - ad * retract_r for hp, ad in zip(homepos, axes_d) - ] - self.toolhead.move(retractpos, hi.retract_speed) - self._set_homing_accel(hi.accel, pre_homing=False) - self._set_homing_current(homing_axes, pre_homing=False) + if hi.use_sensorless_homing and hmove.moved_less_than_dist( + hi.min_home_dist, homing_axes + ): + needs_rehome = True + retract_dist = hi.min_home_dist + sample_retract_dist = hi.min_home_dist + gcode.respond_info( + "Moved less than min_home_dist. Retrying..." + ) + break + # if we use second homing and this is the first home, + # don't use multiple samples as it would be redundant + if not hi.use_sensorless_homing and retract_dist: + break + + _process_sample(trigpos) + + # Perform second home + if (not hi.use_sensorless_homing or needs_rehome) and retract_dist: + if needs_rehome: + logging.info( + "homing:needs rehome: %s", + [("X", "Y", "Z")[axis] for axis in homing_axes], + ) + # Retract + _retract_toolhead(retract_dist, hi.retract_speed) + + distances = [] + retries = 0 + drop_result = hi.drop_first_result + while len(distances) < hi.sample_count: + try: + # Home again + startpos = [ + rp - ad * retract_r + for rp, ad in zip(retractpos, axes_d) + ] + self.toolhead.set_position(startpos) + self._reset_endstop_states(endstops) + + hmove = HomingMove(self.printer, endstops) + trigpos = hmove.homing_move( + homepos, hi.second_homing_speed + ) + + if hmove.check_no_movement() is not None: + raise self.printer.command_error( + "Endstop %s still triggered after retract" + % (hmove.check_no_movement(),) + ) + if ( + hi.use_sensorless_homing + and needs_rehome + and hmove.moved_less_than_dist( + hi.min_home_dist, homing_axes + ) + ): + raise self.printer.command_error( + "Early homing trigger on second home!" + ) + finally: + self._set_homing_accel(hi.accel, pre_homing=False) + + _process_sample(trigpos) + + finally: + self._set_homing_accel(hi.accel, pre_homing=False) + self._set_homing_current(homing_axes, pre_homing=False) + + self.process_homing(distances, homing_axes) + # Signal home operation complete self.toolhead.flush_step_generation() self.trigger_mcu_pos = { sp.stepper_name: sp.trig_pos for sp in hmove.stepper_positions } + + # Process samples if we have any + # No samples are illegal cause that would mean we never home at all, so we check for greater than 1 + if len(distances) > 1: + self.toolhead.wait_moves() + pos = self.toolhead.get_position() + home_pos = self.toolhead.get_position() + calc_adjustment = ( + self._calc_median + if hi.samples_result == "median" + else self._calc_mean + ) + + # calculate the final position + for i in range(0, len(hmove.distance_elapsed)): + pos[i] += ( + calc_adjustment([dist[i] for dist in distances]) + # subtract the last measured distance from the current position so we are at the 0 reference again + # since all distance are relative to that + - distances[-1][i] + ) + pos[i] = round(pos[i], 9) + + for i in homing_axes: + gcode.respond_info( + f"Final homing position for {'XYZ'[i]}: {round(pos[i] + distances[-1][i], 9):.9f}".rstrip( + "0" + ).rstrip(".") + ) + # set the position to what we calculated + self.toolhead.set_position(pos) + if hi.move_toolhead_after_adjusting: + self.printer.lookup_object( + "gcode_move" + ).last_position = home_pos + self.toolhead.move(home_pos, hi.retract_speed) + self.adjust_pos = {} self.printer.send_event("homing:home_rails_end", self, rails) if any(self.adjust_pos.values()): @@ -414,6 +573,83 @@ def home_rails(self, rails, forcepos, movepos): homepos[axis] = newpos[axis] self.toolhead.set_position(homepos) + if hi.retract_dist: + # Retract (again) + self.toolhead.wait_moves() + _retract_toolhead(hi.retract_dist, hi.retract_speed) + self.printer.lookup_object("gcode_move").last_position = retractpos + gcode.run_script_from_command("M400") + + if retries and hi.retry_gcode is not None: + hi.retry_gcode.run_gcode_from_command() + + def process_homing(self, distances, homing_axes): + pass + + +class HomingAccuracy(Homing): + def __init__(self, printer, gcmd): + super().__init__(printer) + self.gcmd = gcmd + + def init_homing(self, hi, homing_axes): + axes = ["XYZ"[i] for i in homing_axes] + for axis in axes: + self.gcmd.respond_info( + "HOMING_ACCURACY for %s" + " (samples=%d retract=%.3f" + " speed=%.1f retract_speed=%.1f)\n" + % ( + axis, + hi.sample_count, + hi.sample_retract_dist, + hi.second_homing_speed, + hi.retract_speed, + ) + ) + + def process_homing_info(self, hi): + drop_first_result = self.gcmd.get_int( + "DROP_FIRST_RESULT", hi.drop_first_result + ) + speed = self.gcmd.get_float("SPEED", hi.second_homing_speed, above=0.0) + sample_retract_speed = self.gcmd.get_float( + "RETRACT_SPEED", hi.sample_retract_speed, above=0.0 + ) + sample_count = self.gcmd.get_int("SAMPLES", 10, minval=1) + sample_retract_dist = self.gcmd.get_float( + "RETRACT_DIST", hi.sample_retract_dist, above=0.0 + ) + homing_info = hi._replace( + drop_first_result=drop_first_result, + speed=speed, + second_homing_speed=speed, + sample_retract_speed=sample_retract_speed, + sample_count=sample_count, + sample_retract_dist=sample_retract_dist, + samples_tolerance=None, + ) + return homing_info + + def process_homing(self, distances, homing_axes): + for i in homing_axes: + dists = [dist[i] for dist in distances] + min_value = min(dists) + max_value = max(dists) + avg_value = self._calc_mean(dists) + median = self._calc_median(dists) + range_value = max_value - min_value + deviation_sum = 0 + for j in range(len(dists)): + deviation_sum += pow(dists[j] - avg_value, 2.0) + sigma = (deviation_sum / len(dists)) ** 0.5 + + self.gcmd.respond_info( + "homing accuracy results: maximum %.6f, minimum %.6f, range %.6f, " + "average %.6f, median %.6f, standard deviation %.6f" + % (max_value, min_value, range_value, avg_value, median, sigma) + ) + class PrinterHoming: def __init__(self, config): @@ -421,6 +657,11 @@ def __init__(self, config): # Register g-code commands gcode = self.printer.lookup_object("gcode") gcode.register_command("G28", self.cmd_G28) + gcode.register_command( + "HOMING_ACCURACY", + self.cmd_HOMING_ACCURACY, + desc=self.cmd_HOMING_ACCURACY_help, + ) def manual_home( self, toolhead, endstops, pos, speed, triggered, check_triggered @@ -454,6 +695,26 @@ def probing_move(self, mcu_probe, pos, speed): ) return epos + cmd_HOMING_ACCURACY_help = "Check the accuracy of your endstops" + + def cmd_HOMING_ACCURACY(self, gcmd): + axes = [] + for pos, axis in enumerate("XYZ"): + if gcmd.get("AXIS") == axis: + axes.append(pos) + homing_state = HomingAccuracy(self.printer, gcmd) + homing_state.set_axes(axes) + kin = self.printer.lookup_object("toolhead").get_kinematics() + try: + kin.home(homing_state) + except self.printer.command_error: + if self.printer.is_shutdown(): + raise self.printer.command_error( + "Homing failed due to printer shutdown" + ) + self.printer.lookup_object("stepper_enable").motor_off() + raise + def cmd_G28(self, gcmd): # Move to origin axes = [] diff --git a/klippy/stepper.py b/klippy/stepper.py index f2b43331c..a6b764462 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -7,6 +7,7 @@ import math from . import chelper +from .extras.danger_options import get_danger_options class error(Exception): @@ -491,6 +492,41 @@ def __init__( self.homing_accel = config.getfloat("homing_accel", None, above=0.0) + self.homing_sample_count = config.getint("homing_samples", 1, minval=1) + self.homing_sample_retract_dist = config.getfloat( + "homing_sample_retract_dist", + self.min_home_dist + + get_danger_options().homing_elapsed_distance_tolerance, + above=self.min_home_dist, + ) + self.homing_sample_retract_speed = config.getfloat( + "homing_sample_retract_speed", self.homing_retract_speed, above=0.0 + ) + atypes = ["median", "average"] + self.homing_samples_result = config.getchoice( + "homing_samples_result", atypes, "average" + ) + self.homing_samples_tolerance = config.getfloat( + "homing_samples_tolerance", 0.100, minval=0.0 + ) + self.homing_samples_retries = config.getint( + "homing_samples_tolerance_retries", 0, minval=0 + ) + self.homing_drop_first_result = config.getboolean( + "homing_drop_first_result", False + ) + self.homing_move_toolhead_after_adjusting = config.getboolean( + "homing_move_toolhead_after_adjusting", False + ) + self.homing_retry_gcode = None + if config.get("homing_retry_gcode", None) is not None: + gcode_macro = config.get_printer().load_object( + config, "gcode_macro" + ) + self.homing_retry_gcode = gcode_macro.load_template( + config, "homing_retry_gcode", "" + ) + if self.homing_positive_dir is None: axis_len = self.position_max - self.position_min if self.position_endstop <= self.position_min + axis_len / 4.0: @@ -538,6 +574,15 @@ def get_homing_info(self): "use_sensorless_homing", "min_home_dist", "accel", + "sample_count", + "sample_retract_dist", + "sample_retract_speed", + "samples_result", + "samples_tolerance", + "samples_retries", + "drop_first_result", + "move_toolhead_after_adjusting", + "retry_gcode", ], )( self.homing_speed, @@ -549,6 +594,15 @@ def get_homing_info(self): self.use_sensorless_homing, self.min_home_dist, self.homing_accel, + self.homing_sample_count, + self.homing_sample_retract_dist, + self.homing_sample_retract_speed, + self.homing_samples_result, + self.homing_samples_tolerance, + self.homing_samples_retries, + self.homing_drop_first_result, + self.homing_move_toolhead_after_adjusting, + self.homing_retry_gcode, ) return homing_info diff --git a/test/klippy/homing_samples.cfg b/test/klippy/homing_samples.cfg new file mode 100644 index 000000000..73be15ce3 --- /dev/null +++ b/test/klippy/homing_samples.cfg @@ -0,0 +1,95 @@ +# Config for multi sample homing testing +[stepper_x] +step_pin: PF0 +dir_pin: PF1 +enable_pin: !PD7 +microsteps: 16 +rotation_distance: 40 +endstop_pin: ^PE5 +position_endstop: 0 +position_max: 200 +homing_speed: 50 +homing_samples: 1 +homing_sample_retract_dist: 5.5 +homing_samples_result: median +homing_samples_tolerance: 100 +homing_samples_tolerance_retries: 3 +homing_drop_first_result: True +homing_move_toolhead_after_adjusting: True +homing_retry_gcode: + RESPOND MSG="Homing retried" + RESPOND MSG="Please rehome y" + +[stepper_y] +step_pin: PF6 +dir_pin: !PF7 +enable_pin: !PF2 +microsteps: 16 +rotation_distance: 40 +endstop_pin: ^PJ1 +position_endstop: 0 +position_max: 200 +homing_speed: 50 +min_home_dist: 2.0 +homing_samples: 1 +homing_sample_retract_dist: 2.5 +homing_samples_result: median +homing_samples_tolerance: 100 +homing_samples_tolerance_retries: 3 +homing_drop_first_result: True +homing_move_toolhead_after_adjusting: True +homing_retry_gcode: + G28 X + +[stepper_z] +step_pin: PL3 +dir_pin: PL1 +enable_pin: !PK0 +microsteps: 16 +rotation_distance: 8 +endstop_pin: ^PD3 +position_endstop: 0.5 +position_max: 200 +homing_samples: 3 +homing_samples_result: median +homing_samples_tolerance: 100 +homing_samples_tolerance_retries: 3 +homing_drop_first_result: True +homing_move_toolhead_after_adjusting: True + +[extruder] +step_pin: PA4 +dir_pin: PA6 +enable_pin: !PA2 +microsteps: 16 +rotation_distance: 33.5 +nozzle_diameter: 0.500 +filament_diameter: 3.500 +heater_pin: PB4 +sensor_type: EPCOS 100K B57560G104F +sensor_pin: PK5 +control: pid +pid_Kp: 22.2 +pid_Ki: 1.08 +pid_Kd: 114 +min_temp: 0 +max_temp: 210 +per_move_pressure_advance: True + +[heater_bed] +heater_pin: PH5 +sensor_type: EPCOS 100K B57560G104F +sensor_pin: PK6 +control: watermark +min_temp: 0 +max_temp: 130 + +[mcu] +serial: /dev/ttyACM0 + +[printer] +kinematics: corexy +max_velocity: 300 +max_accel: 3000 +max_z_velocity: 5 +max_z_accel: 100 \ No newline at end of file diff --git a/test/klippy/homing_samples.test b/test/klippy/homing_samples.test new file mode 100644 index 000000000..f76c8d1a3 --- /dev/null +++ b/test/klippy/homing_samples.test @@ -0,0 +1,12 @@ +# Test cases for multi sample homing +CONFIG homing_samples.cfg +DICTIONARY atmega2560.dict + +# First home the printer +G90 +G28 + +# Perform a dummy move +G1 X10 F6000 + +HOMING_ACCURACY AXIS=X \ No newline at end of file