From 929733f0a7ffc660e0e47f131b7f3ceb2d46efbd Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Tue, 4 Sep 2018 16:48:36 -0400 Subject: [PATCH] delta_calibrate: Support save/restore of delta_calibrate state Support using SAVE_CONFIG to store the results of DELTA_CALIBRATE to the printer config file. Store the low level probe measurements in the config as well. Signed-off-by: Kevin O'Connor --- klippy/extras/delta_calibrate.py | 69 +++++++++++++++++++++++++++----- 1 file changed, 59 insertions(+), 10 deletions(-) diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py index b065da74..97c75466 100644 --- a/klippy/extras/delta_calibrate.py +++ b/klippy/extras/delta_calibrate.py @@ -56,6 +56,17 @@ def get_stable_position(stepper_position, delta_params): for sd, ep, sp in zip( dp.stepdists, dp.abs_endstops, stepper_position)] +# Load a stable position from a config entry +def load_config_stable(config, option): + spos = config.get(option) + try: + sa, sb, sc = map(float, spos.split(',')) + except: + msg = "Unable to parse stable position '%s'" % (spos,) + logging.exception(msg) + raise config.error(msg) + return sa, sb, sc + ###################################################################### # Delta Calibrate class @@ -76,11 +87,37 @@ class DeltaCalibrate: points.append((math.cos(r) * dist, math.sin(r) * dist)) self.probe_helper = probe.ProbePointsHelper( config, self, default_points=points) + # Restore probe stable positions + self.last_probe_positions = [] + for i in range(999): + height = config.getfloat("height%d" % (i,), None) + if height is None: + break + height_pos = load_config_stable(config, "height%d_pos" % (i,)) + self.last_probe_positions.append((height, height_pos)) # Register DELTA_CALIBRATE command self.gcode = self.printer.lookup_object('gcode') self.gcode.register_command( 'DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE, desc=self.cmd_DELTA_CALIBRATE_help) + def save_state(self, probe_positions, params): + # Save main delta parameters + configfile = self.printer.lookup_object('configfile') + configfile.set('printer', 'delta_radius', "%.6f" % (params['radius'])) + for axis in 'abc': + configfile.set('stepper_'+axis, 'angle', + "%.6f" % (params['angle_'+axis],)) + configfile.set('stepper_'+axis, 'arm_length', + "%.6f" % (params['arm_'+axis],)) + configfile.set('stepper_'+axis, 'position_endstop', + "%.6f" % (params['endstop_'+axis],)) + # Save probe stable positions + section = 'delta_calibrate' + configfile.remove_section(section) + for i, (z_offset, spos) in enumerate(probe_positions): + configfile.set(section, "height%d" % (i,), z_offset) + configfile.set(section, "height%d_pos" % (i,), + "%d,%d,%d" % tuple(spos)) cmd_DELTA_CALIBRATE_help = "Delta calibration script" def cmd_DELTA_CALIBRATE(self, params): self.gcode.run_script_from_command("G28") @@ -89,43 +126,55 @@ class DeltaCalibrate: kin = self.printer.lookup_object('toolhead').get_kinematics() return [s.get_commanded_position() for s in kin.get_steppers()] def finalize(self, offsets, positions): + # Convert positions into (z_offset, stable_position) pairs z_offset = offsets[2] kin = self.printer.lookup_object('toolhead').get_kinematics() + delta_params = build_delta_params(kin.get_calibrate_params()) + probe_positions = [(z_offset, get_stable_position(p, delta_params)) + for p in positions] + # Perform analysis + self.calculate_params(probe_positions) + def calculate_params(self, probe_positions): + # Setup for coordinate descent analysis + kin = self.printer.lookup_object('toolhead').get_kinematics() params = kin.get_calibrate_params() orig_delta_params = build_delta_params(params) - stable_positions = [get_stable_position(p, orig_delta_params) - for p in positions] logging.info("Calculating delta_calibrate with: %s\n" "Initial delta_calibrate parameters: %s", - stable_positions, params) + probe_positions, params) adj_params = ('radius', 'angle_a', 'angle_b', 'endstop_a', 'endstop_b', 'endstop_c') + # Perform coordinate descent def delta_errorfunc(params): delta_params = build_delta_params(params) total_error = 0. - for stable_pos in stable_positions: + for z_offset, stable_pos in probe_positions: x, y, z = get_position_from_stable(stable_pos, delta_params) total_error += (z - z_offset)**2 return total_error new_params = mathutil.coordinate_descent( adj_params, params, delta_errorfunc) + # Log and report results logging.info("Calculated delta_calibrate parameters: %s", new_params) new_delta_params = build_delta_params(new_params) - for spos in stable_positions: - logging.info("orig: %s new: %s", - get_position_from_stable(spos, orig_delta_params), - get_position_from_stable(spos, new_delta_params)) + for z_offset, spos in probe_positions: + logging.info("height orig: %.6f new: %.6f goal: %.6f", + get_position_from_stable(spos, orig_delta_params)[2], + get_position_from_stable(spos, new_delta_params)[2], + z_offset) self.gcode.respond_info( "stepper_a: position_endstop: %.6f angle: %.6f\n" "stepper_b: position_endstop: %.6f angle: %.6f\n" "stepper_c: position_endstop: %.6f angle: %.6f\n" "delta_radius: %.6f\n" - "To use these parameters, update the printer config file with\n" - "the above and then issue a RESTART command" % ( + "The SAVE_CONFIG command will update the printer config file\n" + "with these parameters and restart the printer." % ( new_params['endstop_a'], new_params['angle_a'], new_params['endstop_b'], new_params['angle_b'], new_params['endstop_c'], new_params['angle_c'], new_params['radius'])) + # Store results for SAVE_CONFIG + self.save_state(probe_positions, new_params) def load_config(config): return DeltaCalibrate(config)