diff options
| -rw-r--r-- | klippy/extras/delta_calibrate.py | 84 | ||||
| -rw-r--r-- | klippy/kinematics/delta.py | 42 |
2 files changed, 82 insertions, 44 deletions
diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py index 99b59490..b065da74 100644 --- a/klippy/extras/delta_calibrate.py +++ b/klippy/extras/delta_calibrate.py @@ -3,9 +3,64 @@ # 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 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. + +# Storage helper for delta parameters +DeltaParams = collections.namedtuple('DeltaParams', [ + 'radius', 'angles', 'arms', 'endstops', 'stepdists', + 'towers', 'abs_endstops']) + +# Generate delta_params from delta configuration parameters +def build_delta_params(params): + radius = params['radius'] + angles = [params['angle_'+a] for a in 'abc'] + arms = [params['arm_'+a] for a in 'abc'] + endstops = [params['endstop_'+a] for a in 'abc'] + 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 angles] + towers = [(math.cos(a) * radius, math.sin(a) * radius) + for a in radian_angles] + # Calculate the absolute Z height of each tower endstop + radius2 = radius**2 + abs_endstops = [e + math.sqrt(a**2 - radius2) + for e, a in zip(endstops, arms)] + return DeltaParams(radius, angles, arms, endstops, stepdists, + towers, abs_endstops) + +# Return cartesian coordinates for the given stable_positions when the +# given delta_params are used. +def get_position_from_stable(stable_position, delta_params): + dp = delta_params + sphere_coords = [ + (t[0], t[1], es - sp * sd) + for sd, t, es, sp in zip( + dp.stepdists, dp.towers, dp.abs_endstops, stable_position) ] + return mathutil.trilateration(sphere_coords, [a**2 for a in dp.arms]) + +# Return a stable position from the nominal delta tower positions +def get_stable_position(stepper_position, delta_params): + dp = delta_params + return [int((ep - sp) / sd + .5) + for sd, ep, sp in zip( + dp.stepdists, dp.abs_endstops, stepper_position)] + + +###################################################################### +# Delta Calibrate class +###################################################################### + class DeltaCalibrate: def __init__(self, config): self.printer = config.get_printer() @@ -32,27 +87,34 @@ class DeltaCalibrate: self.probe_helper.start_probe() def get_probed_position(self): kin = self.printer.lookup_object('toolhead').get_kinematics() - return kin.get_stable_position() + return [s.get_commanded_position() for s in kin.get_steppers()] def finalize(self, offsets, positions): z_offset = offsets[2] kin = self.printer.lookup_object('toolhead').get_kinematics() - logging.info("Calculating delta_calibrate with: %s", positions) params = kin.get_calibrate_params() - logging.info("Initial delta_calibrate parameters: %s", params) - adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius', - 'angle_a', 'angle_b') + 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) + adj_params = ('radius', 'angle_a', 'angle_b', + 'endstop_a', 'endstop_b', 'endstop_c') def delta_errorfunc(params): + delta_params = build_delta_params(params) total_error = 0. - for x, y, z in kin.get_positions_from_stable(positions, params): + for stable_pos in stable_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) logging.info("Calculated delta_calibrate parameters: %s", new_params) - old_positions = kin.get_positions_from_stable(positions, params) - new_positions = kin.get_positions_from_stable(positions, new_params) - for oldpos, newpos in zip(old_positions, new_positions): - logging.info("orig: %s new: %s", oldpos, newpos) + 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)) self.gcode.respond_info( "stepper_a: position_endstop: %.6f angle: %.6f\n" "stepper_b: position_endstop: %.6f angle: %.6f\n" diff --git a/klippy/kinematics/delta.py b/klippy/kinematics/delta.py index 5a7a6ca6..84f8db78 100644 --- a/klippy/kinematics/delta.py +++ b/klippy/kinematics/delta.py @@ -12,8 +12,7 @@ SLOW_RATIO = 3. class DeltaKinematics: def __init__(self, toolhead, config): # Setup tower rails - stepper_configs = [config.getsection('stepper_' + n) - for n in ['a', 'b', 'c']] + stepper_configs = [config.getsection('stepper_' + a) for a in 'abc'] rail_a = stepper.PrinterRail( stepper_configs[0], need_position_minmax = False) a_endstop = rail_a.get_homing_info().position_endstop @@ -31,9 +30,9 @@ class DeltaKinematics: sconfig.getfloat('arm_length', arm_length_a, above=radius) for sconfig in stepper_configs] self.arm2 = [arm**2 for arm in arm_lengths] - self.endstops = [(rail.get_homing_info().position_endstop - + math.sqrt(arm2 - radius**2)) - for rail, arm2 in zip(self.rails, self.arm2)] + self.abs_endstops = [(rail.get_homing_info().position_endstop + + math.sqrt(arm2 - radius**2)) + for rail, arm2 in zip(self.rails, self.arm2)] # Setup boundary checks self.need_motor_enable = self.need_home = True self.limit_xy2 = -1. @@ -41,7 +40,7 @@ class DeltaKinematics: for rail in self.rails]) self.min_z = config.getfloat('minimum_z_position', 0, maxval=self.max_z) self.limit_z = min([ep - arm - for ep, arm in zip(self.endstops, arm_lengths)]) + for ep, arm in zip(self.abs_endstops, arm_lengths)]) logging.info( "Delta max build height %.2fmm (radius tapered above %.2fmm)" % ( self.max_z, self.limit_z)) @@ -115,7 +114,7 @@ class DeltaKinematics: homing_speed/2.0, second_home=True) # Set final homed position spos = [ep + rail.get_homed_offset() - for ep, rail in zip(self.endstops, self.rails)] + for ep, rail in zip(self.abs_endstops, self.rails)] homing_state.set_homed_position(self._actuator_to_cartesian(spos)) def motor_off(self, print_time): self.limit_xy2 = -1. @@ -160,38 +159,15 @@ class DeltaKinematics: self._check_motor_enable(print_time) for rail in self.rails: rail.step_itersolve(move.cmove) - # Helper functions for DELTA_CALIBRATE script - def get_stable_position(self): - steppers = [rail.get_steppers()[0] for rail in self.rails] - return [int((ep - s.get_commanded_position()) / s.get_step_dist() + .5) - for ep, s in zip(self.endstops, steppers)] + # Helper function for DELTA_CALIBRATE script def get_calibrate_params(self): out = { 'radius': self.radius } for i, axis in enumerate('abc'): rail = self.rails[i] - out['endstop_'+axis] = rail.get_homing_info().position_endstop - out['stepdist_'+axis] = rail.get_steppers()[0].get_step_dist() out['angle_'+axis] = self.angles[i] out['arm_'+axis] = self.arm_lengths[i] - return out - def get_positions_from_stable(self, stable_positions, params): - angle_names = ['angle_a', 'angle_b', 'angle_c'] - angles = [math.radians(params[an]) for an in angle_names] - radius = params['radius'] - radius2 = radius**2 - towers = [(math.cos(a) * radius, math.sin(a) * radius) for a in angles] - arm2 = [params[an]**2 for an in ['arm_a', 'arm_b', 'arm_c']] - stepdist_names = ['stepdist_a', 'stepdist_b', 'stepdist_c'] - stepdists = [params[sn] for sn in stepdist_names] - endstop_names = ['endstop_a', 'endstop_b', 'endstop_c'] - endstops = [params[en] + math.sqrt(a2 - radius2) - for en, a2 in zip(endstop_names, arm2)] - out = [] - for spos in stable_positions: - sphere_coords = [ - (t[0], t[1], es - sp * sd) - for t, es, sd, sp in zip(towers, endstops, stepdists, spos) ] - out.append(mathutil.trilateration(sphere_coords, arm2)) + out['endstop_'+axis] = rail.get_homing_info().position_endstop + out['stepdist_'+axis] = rail.get_steppers()[0].get_step_dist() return out def load_kinematics(toolhead, config): |