diff options
Diffstat (limited to 'klippy/extras')
| -rw-r--r-- | klippy/extras/delta_calibrate.py | 76 | ||||
| -rw-r--r-- | klippy/extras/probe.py | 86 |
2 files changed, 162 insertions, 0 deletions
diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py new file mode 100644 index 00000000..5c69aa8d --- /dev/null +++ b/klippy/extras/delta_calibrate.py @@ -0,0 +1,76 @@ +# Delta calibration support +# +# Copyright (C) 2017-2018 Kevin O'Connor <kevin@koconnor.net> +# +# This file may be distributed under the terms of the GNU GPLv3 license. +import math, logging +import probe, delta + +class DeltaCalibrate: + def __init__(self, config): + self.printer = config.get_printer() + if config.getsection('printer').get('kinematics') != 'delta': + raise config.error("Delta calibrate is only for delta printers") + self.radius = config.getfloat('radius', above=0.) + self.speed = config.getfloat('speed', 50., above=0.) + self.horizontal_move_z = config.getfloat('horizontal_move_z', 5.) + self.probe_z_offset = config.getfloat('probe_z_offset', 0.) + self.manual_probe = config.getboolean('manual_probe', None) + if self.manual_probe is None: + self.manual_probe = not config.has_section('probe') + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command( + 'DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE, + desc=self.cmd_DELTA_CALIBRATE_help) + cmd_DELTA_CALIBRATE_help = "Delta calibration script" + def cmd_DELTA_CALIBRATE(self, params): + # Setup probe points + points = [(0., 0.)] + scatter = [.95, .90, .85, .70, .75, .80] + for i in range(6): + r = math.radians(90. + 60. * i) + dist = self.radius * scatter[i] + points.append((math.cos(r) * dist, math.sin(r) * dist)) + # Probe them + self.gcode.run_script("G28") + probe.ProbePointsHelper(self.printer, points, self.horizontal_move_z, + self.speed, self.manual_probe, self) + def get_position(self): + kin = self.printer.lookup_object('toolhead').get_kinematics() + return kin.get_stable_position() + def finalize(self, positions): + kin = self.printer.lookup_object('toolhead').get_kinematics() + logging.debug("Got: %s", positions) + params = kin.get_calibrate_params() + logging.debug("Params: %s", params) + adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius', + 'angle_a', 'angle_b') + def delta_errorfunc(params): + total_error = 0. + for spos in positions: + x, y, z = delta.get_position_from_stable(spos, params) + total_error += (z - self.probe_z_offset)**2 + return total_error + new_params = probe.coordinate_descent( + adj_params, params, delta_errorfunc) + logging.debug("Got2: %s", new_params) + for spos in positions: + logging.debug("orig: %s new: %s", + delta.get_position_from_stable(spos, params), + delta.get_position_from_stable(spos, new_params)) + 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" + "radius: %.6f\n" + "To use these parameters, update the printer config file with\n" + "the above and then issue a RESTART command" % ( + 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'])) + +def load_config(config): + if config.get_name() != 'delta_calibrate': + raise config.error("Invalid delta_calibrate config name") + return DeltaCalibrate(config) diff --git a/klippy/extras/probe.py b/klippy/extras/probe.py index 73bdfafa..aeebf725 100644 --- a/klippy/extras/probe.py +++ b/klippy/extras/probe.py @@ -3,6 +3,7 @@ # Copyright (C) 2017-2018 Kevin O'Connor <kevin@koconnor.net> # # This file may be distributed under the terms of the GNU GPLv3 license. +import logging import homing class PrinterProbe: @@ -48,6 +49,91 @@ class PrinterProbe: self.gcode.respond_info( "probe: %s" % (["open", "TRIGGERED"][not not res],)) +# Helper code that can probe a series of points and report the +# position at each point. +class ProbePointsHelper: + def __init__(self, printer, probe_points, horizontal_move_z, speed, + manual_probe, callback): + self.printer = printer + self.probe_points = probe_points + self.horizontal_move_z = horizontal_move_z + self.speed = speed + self.callback = callback + self.toolhead = self.printer.lookup_object('toolhead') + self.results = [] + self.busy = True + self.gcode = self.printer.lookup_object('gcode') + self.gcode.register_command( + 'NEXT', self.cmd_NEXT, desc=self.cmd_NEXT_help) + # Begin probing + self.move_next() + if not manual_probe: + while self.busy: + self.gcode.run_script("PROBE") + self.cmd_NEXT({}) + def move_next(self): + x, y = self.probe_points[len(self.results)] + curpos = self.toolhead.get_position() + curpos[0] = x + curpos[1] = y + curpos[2] = self.horizontal_move_z + self.toolhead.move(curpos, self.speed) + self.gcode.reset_last_position() + cmd_NEXT_help = "Move to the next XY position to probe" + def cmd_NEXT(self, params): + # Record current position + self.toolhead.wait_moves() + self.results.append(self.callback.get_position()) + # Move to next position + curpos = self.toolhead.get_position() + curpos[2] = self.horizontal_move_z + self.toolhead.move(curpos, self.speed) + if len(self.results) == len(self.probe_points): + self.toolhead.get_last_move_time() + self.finalize(True) + return + self.move_next() + def finalize(self, success): + self.busy = False + self.gcode.reset_last_position() + self.gcode.register_command('NEXT', None) + if success: + self.callback.finalize(self.results) + +# Helper code that implements coordinate descent +def coordinate_descent(adj_params, params, error_func): + # Define potential changes + params = dict(params) + dp = {param_name: 1. for param_name in adj_params} + # Calculate the error + best_err = error_func(params) + + threshold = 0.00001 + rounds = 0 + + while sum(dp.values()) > threshold and rounds < 10000: + rounds += 1 + for param_name in adj_params: + orig = params[param_name] + params[param_name] = orig + dp[param_name] + err = error_func(params) + if err < best_err: + # There was some improvement + best_err = err + dp[param_name] *= 1.1 + continue + params[param_name] = orig - dp[param_name] + err = error_func(params) + if err < best_err: + # There was some improvement + best_err = err + dp[param_name] *= 1.1 + continue + params[param_name] = orig + dp[param_name] *= 0.9 + logging.debug("best_err: %s rounds: %d", best_err, rounds) + return params + def load_config(config): if config.get_name() != 'probe': raise config.error("Invalid probe config name") |