3D-printing/klipper
1
0
Fork 0
mirror of https://github.com/Klipper3d/klipper.git synced 2025-07-23 22:54:10 -06:00
Code Issues Projects Releases Packages Wiki Activity Actions 2

delta: Move "stable position" logic to delta_calibrate.py

Browse source 
Move the "stable position" logic from the delta.py kinematics code to
the delta_calibrate.py calibration code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2018-09-03 13:34:27 -04:00 • committed by KevinOConnor
parent d48e8ea162
commit ed0882dc10
2 changed files with 82 additions and 44 deletions
Download patch file Download diff file Expand all files Collapse all files

84
klippy/extras/delta_calibrate.py
View file

@ -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"