From 27e6b533bc7f41d6c4b2e9e52fe49f954c2f6bab Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 5 Dec 2019 10:44:08 -0500 Subject: [PATCH] delta: Move low-level delta calibration to delta.py Move the linear delta specific calibration code from delta_calibrate.py to delta.py. Signed-off-by: Kevin O'Connor --- klippy/extras/delta_calibrate.py | 80 ++++---------------------------- klippy/kinematics/delta.py | 68 ++++++++++++++++++++++++++- 2 files changed, 76 insertions(+), 72 deletions(-) diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py index 02507465..75a06278 100644 --- a/klippy/extras/delta_calibrate.py +++ b/klippy/extras/delta_calibrate.py @@ -6,76 +6,11 @@ import math, logging, collections import probe, mathutil - -###################################################################### -# Delta "stable position" coordinates -###################################################################### - # A "stable position" is a 3-tuple containing the number of steps # taken since hitting the endstop on each delta tower. Delta # calibration uses this coordinate system because it allows a position # to be described independent of the software parameters. -class DeltaCalibration: - def __init__(self, params): - self.params = dict(params) - self.radius = params['radius'] - self.angles = [params['angle_'+a] for a in 'abc'] - self.arms = [params['arm_'+a] for a in 'abc'] - self.endstops = [params['endstop_'+a] for a in 'abc'] - self.stepdists = [params['stepdist_'+a] for a in 'abc'] - # Calculate the XY cartesian coordinates of the delta towers - radian_angles = [math.radians(a) for a in self.angles] - self.towers = [(math.cos(a) * self.radius, math.sin(a) * self.radius) - for a in radian_angles] - # Calculate the absolute Z height of each tower endstop - radius2 = self.radius**2 - self.abs_endstops = [e + math.sqrt(a**2 - radius2) - for e, a in zip(self.endstops, self.arms)] - def get_position_from_stable(self, stable_position): - # Return cartesian coordinates for the given stable_position - sphere_coords = [ - (t[0], t[1], es - sp * sd) - for sd, t, es, sp in zip(self.stepdists, self.towers, - self.abs_endstops, stable_position) ] - return mathutil.trilateration(sphere_coords, [a**2 for a in self.arms]) - def calc_stable_position(self, coord): - # Return a stable_position from a cartesian coordinate - steppos = [ - math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2] - for t, a in zip(self.towers, self.arms) ] - return [(ep - sp) / sd - for sd, ep, sp in zip(self.stepdists, - self.abs_endstops, steppos)] - def coordinate_descent_params(self, is_extended): - adj_params = ('radius', 'angle_a', 'angle_b', - 'endstop_a', 'endstop_b', 'endstop_c') - if is_extended: - adj_params += ('arm_a', 'arm_b', 'arm_c') - return adj_params, self.params - def new_calibration(self, params): - return DeltaCalibration(params) - def save_state(self, configfile): - params = self.params - 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],)) - gcode = configfile.get_printer().lookup_object("gcode") - gcode.respond_info( - "stepper_a: position_endstop: %.6f angle: %.6f arm: %.6f\n" - "stepper_b: position_endstop: %.6f angle: %.6f arm: %.6f\n" - "stepper_c: position_endstop: %.6f angle: %.6f arm: %.6f\n" - "delta_radius: %.6f\n" - % (params['endstop_a'], params['angle_a'], params['arm_a'], - params['endstop_b'], params['angle_b'], params['arm_b'], - params['endstop_c'], params['angle_c'], params['arm_c'], - params['radius'])) - # Load a stable position from a config entry def load_config_stable(config, option): spos = config.get(option) @@ -149,8 +84,8 @@ def measurements_to_distances(measured_params, delta_params): 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.printer.register_event_handler("klippy:connect", + self.handle_connect) # Calculate default probing points radius = config.getfloat('radius', above=0.) points = [(0., 0.)] @@ -186,6 +121,11 @@ class DeltaCalibrate: desc=self.cmd_DELTA_CALIBRATE_help) self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE, desc=self.cmd_DELTA_ANALYZE_help) + def handle_connect(self): + kin = self.printer.lookup_object('toolhead').get_kinematics() + if not hasattr(kin, "get_calibration"): + raise self.printer.config_error( + "Delta calibrate is only for delta printers") def save_state(self, probe_positions, distances, delta_params): # Save main delta parameters configfile = self.printer.lookup_object('configfile') @@ -208,7 +148,7 @@ class DeltaCalibrate: # Convert positions into (z_offset, stable_position) pairs z_offset = offsets[2] kin = self.printer.lookup_object('toolhead').get_kinematics() - delta_params = DeltaCalibration(kin.get_calibrate_params()) + delta_params = kin.get_calibration() probe_positions = [(z_offset, delta_params.calc_stable_position(p)) for p in positions] # Perform analysis @@ -216,7 +156,7 @@ class DeltaCalibrate: def calculate_params(self, probe_positions, distances): # Setup for coordinate descent analysis kin = self.printer.lookup_object('toolhead').get_kinematics() - orig_delta_params = odp = DeltaCalibration(kin.get_calibrate_params()) + orig_delta_params = odp = kin.get_calibration() adj_params, params = odp.coordinate_descent_params(distances) logging.info("Calculating delta_calibrate with:\n%s\n%s\n" "Initial delta_calibrate parameters: %s", @@ -276,7 +216,7 @@ class DeltaCalibrate: raise self.gcode.error("Not all measurements provided") else: kin = self.printer.lookup_object('toolhead').get_kinematics() - delta_params = DeltaCalibration(kin.get_calibrate_params()) + delta_params = kin.get_calibration() distances = measurements_to_distances( self.delta_analyze_entry, delta_params) if not self.last_probe_positions: diff --git a/klippy/kinematics/delta.py b/klippy/kinematics/delta.py index 73a987d3..f51db5ad 100644 --- a/klippy/kinematics/delta.py +++ b/klippy/kinematics/delta.py @@ -148,7 +148,7 @@ class DeltaKinematics: return {'homed_axes': '' if self.need_home else 'xyz'} # Helper function for DELTA_CALIBRATE script - def get_calibrate_params(self): + def get_calibration(self): out = { 'radius': self.radius } for i, axis in enumerate('abc'): rail = self.rails[i] @@ -156,7 +156,71 @@ class DeltaKinematics: out['arm_'+axis] = self.arm_lengths[i] out['endstop_'+axis] = rail.get_homing_info().position_endstop out['stepdist_'+axis] = rail.get_steppers()[0].get_step_dist() - return out + return DeltaCalibration(out) + +# Delta parameter calibration for DELTA_CALIBRATE tool +class DeltaCalibration: + def __init__(self, params): + self.params = dict(params) + self.radius = params['radius'] + self.angles = [params['angle_'+a] for a in 'abc'] + self.arms = [params['arm_'+a] for a in 'abc'] + self.endstops = [params['endstop_'+a] for a in 'abc'] + self.stepdists = [params['stepdist_'+a] for a in 'abc'] + # Calculate the XY cartesian coordinates of the delta towers + radian_angles = [math.radians(a) for a in self.angles] + self.towers = [(math.cos(a) * self.radius, math.sin(a) * self.radius) + for a in radian_angles] + # Calculate the absolute Z height of each tower endstop + radius2 = self.radius**2 + self.abs_endstops = [e + math.sqrt(a**2 - radius2) + for e, a in zip(self.endstops, self.arms)] + def new_calibration(self, params): + # Create a new calibration object with the coordinate_descent params + return DeltaCalibration(params) + def get_position_from_stable(self, stable_position): + # Return cartesian coordinates for the given stable_position + sphere_coords = [ + (t[0], t[1], es - sp * sd) + for sd, t, es, sp in zip(self.stepdists, self.towers, + self.abs_endstops, stable_position) ] + return mathutil.trilateration(sphere_coords, [a**2 for a in self.arms]) + def calc_stable_position(self, coord): + # Return a stable_position from a cartesian coordinate + steppos = [ + math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2] + for t, a in zip(self.towers, self.arms) ] + return [(ep - sp) / sd + for sd, ep, sp in zip(self.stepdists, + self.abs_endstops, steppos)] + def coordinate_descent_params(self, is_extended): + # Determine adjustment parameters (for use with coordinate_descent) + adj_params = ('radius', 'angle_a', 'angle_b', + 'endstop_a', 'endstop_b', 'endstop_c') + if is_extended: + adj_params += ('arm_a', 'arm_b', 'arm_c') + return adj_params, self.params + def save_state(self, configfile): + # Save the current parameters (for use with SAVE_CONFIG) + params = self.params + 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],)) + gcode = configfile.get_printer().lookup_object("gcode") + gcode.respond_info( + "stepper_a: position_endstop: %.6f angle: %.6f arm: %.6f\n" + "stepper_b: position_endstop: %.6f angle: %.6f arm: %.6f\n" + "stepper_c: position_endstop: %.6f angle: %.6f arm: %.6f\n" + "delta_radius: %.6f\n" + % (params['endstop_a'], params['angle_a'], params['arm_a'], + params['endstop_b'], params['angle_b'], params['arm_b'], + params['endstop_c'], params['angle_c'], params['arm_c'], + params['radius'])) def load_kinematics(toolhead, config): return DeltaKinematics(toolhead, config)