diff options
| -rw-r--r-- | klippy/extras/delta_calibrate.py | 14 | ||||
| -rw-r--r-- | klippy/kinematics/delta.py | 28 |
2 files changed, 22 insertions, 20 deletions
diff --git a/klippy/extras/delta_calibrate.py b/klippy/extras/delta_calibrate.py index 42104d29..f8f87d9e 100644 --- a/klippy/extras/delta_calibrate.py +++ b/klippy/extras/delta_calibrate.py @@ -4,7 +4,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import math, logging -import probe, mathutil, kinematics.delta +import probe, mathutil class DeltaCalibrate: def __init__(self, config): @@ -40,20 +40,18 @@ class DeltaCalibrate: logging.info("Initial delta_calibrate parameters: %s", params) adj_params = ('endstop_a', 'endstop_b', 'endstop_c', 'radius', 'angle_a', 'angle_b') - get_position_from_stable = kinematics.delta.get_position_from_stable def delta_errorfunc(params): total_error = 0. - for spos in positions: - x, y, z = get_position_from_stable(spos, params) + for x, y, z in kin.get_positions_from_stable(positions, 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) - for spos in positions: - logging.info("orig: %s new: %s", - get_position_from_stable(spos, params), - get_position_from_stable(spos, 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) 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 b584f9c9..0c66c791 100644 --- a/klippy/kinematics/delta.py +++ b/klippy/kinematics/delta.py @@ -174,18 +174,22 @@ class DeltaKinematics: 'angle_c': self.angles[2], 'radius': self.radius, 'arm_a': self.arm_lengths[0], 'arm_b': self.arm_lengths[1], 'arm_c': self.arm_lengths[2] } - -def get_position_from_stable(spos, params): - angles = [params['angle_a'], params['angle_b'], params['angle_c']] - radius = params['radius'] - radius2 = radius**2 - towers = [(math.cos(angle) * radius, math.sin(angle) * radius) - for angle in map(math.radians, angles)] - arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]] - endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']] - sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p) - for t, es, a2, p in zip(towers, endstops, arm2, spos)] - return mathutil.trilateration(sphere_coords, arm2) + 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']] + 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) + for t, es, sp in zip(towers, endstops, spos)] + out.append(mathutil.trilateration(sphere_coords, arm2)) + return out def load_kinematics(toolhead, config): return DeltaKinematics(toolhead, config) |