3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

delta_calibrate: Move stable position conversion to its own class 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2019-12-05 09:03:37 -05:00
parent 90bc1679a2
commit fdfa26edd6
2 changed files with 86 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
klippy/configfile.py
View File

@ -91,6 +91,8 @@ class PrinterConfig:
gcode = self.printer.lookup_object('gcode') gcode = self.printer.lookup_object('gcode')
gcode.register_command("SAVE_CONFIG", self.cmd_SAVE_CONFIG, gcode.register_command("SAVE_CONFIG", self.cmd_SAVE_CONFIG,
desc=self.cmd_SAVE_CONFIG_help) desc=self.cmd_SAVE_CONFIG_help)
def get_printer(self):
return self.printer
def _read_config_file(self, filename): def _read_config_file(self, filename):
try: try:
f = open(filename, 'rb') f = open(filename, 'rb')

171
klippy/extras/delta_calibrate.py
View File

@ -1,6 +1,6 @@
# Delta calibration support # Delta calibration support
# #
# Copyright (C) 2017-2018 Kevin O'Connor <kevin@koconnor.net> # Copyright (C) 2017-2019 Kevin O'Connor <kevin@koconnor.net>
# #
# This file may be distributed under the terms of the GNU GPLv3 license. # This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging, collections import math, logging, collections
@ -16,47 +16,65 @@ import probe, mathutil
# calibration uses this coordinate system because it allows a position # calibration uses this coordinate system because it allows a position
# to be described independent of the software parameters. # to be described independent of the software parameters.
# Storage helper for delta parameters class DeltaCalibration:
DeltaParams = collections.namedtuple('DeltaParams', [ def __init__(self, params):
'radius', 'angles', 'arms', 'endstops', 'stepdists', self.params = dict(params)
'towers', 'abs_endstops']) self.radius = params['radius']
self.angles = [params['angle_'+a] for a in 'abc']
# Generate delta_params from delta configuration parameters self.arms = [params['arm_'+a] for a in 'abc']
def build_delta_params(params): self.endstops = [params['endstop_'+a] for a in 'abc']
radius = params['radius'] self.stepdists = [params['stepdist_'+a] for a in 'abc']
angles = [params['angle_'+a] for a in 'abc'] # Calculate the XY cartesian coordinates of the delta towers
arms = [params['arm_'+a] for a in 'abc'] radian_angles = [math.radians(a) for a in self.angles]
endstops = [params['endstop_'+a] for a in 'abc'] self.towers = [(math.cos(a) * self.radius, math.sin(a) * self.radius)
stepdists = [params['stepdist_'+a] for a in 'abc'] for a in radian_angles]
# Calculate the XY cartesian coordinates of the delta towers # Calculate the absolute Z height of each tower endstop
radian_angles = [math.radians(a) for a in angles] radius2 = self.radius**2
towers = [(math.cos(a) * radius, math.sin(a) * radius) self.abs_endstops = [e + math.sqrt(a**2 - radius2)
for a in radian_angles] for e, a in zip(self.endstops, self.arms)]
# Calculate the absolute Z height of each tower endstop def get_position_from_stable(self, stable_position):
radius2 = radius**2 # Return cartesian coordinates for the given stable_position
abs_endstops = [e + math.sqrt(a**2 - radius2) sphere_coords = [
for e, a in zip(endstops, arms)] (t[0], t[1], es - sp * sd)
return DeltaParams(radius, angles, arms, endstops, stepdists, for sd, t, es, sp in zip(self.stepdists, self.towers,
towers, abs_endstops) self.abs_endstops, stable_position) ]
return mathutil.trilateration(sphere_coords, [a**2 for a in self.arms])
# Return cartesian coordinates for the given stable_positions when the def calc_stable_position(self, coord):
# given delta_params are used. # Return a stable_position from a cartesian coordinate
def get_position_from_stable(stable_position, delta_params): steppos = [
dp = delta_params math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2]
sphere_coords = [ for t, a in zip(self.towers, self.arms) ]
(t[0], t[1], es - sp * sd) return [(ep - sp) / sd
for sd, t, es, sp in zip( for sd, ep, sp in zip(self.stepdists,
dp.stepdists, dp.towers, dp.abs_endstops, stable_position) ] self.abs_endstops, steppos)]
return mathutil.trilateration(sphere_coords, [a**2 for a in dp.arms]) def coordinate_descent_params(self, is_extended):
adj_params = ('radius', 'angle_a', 'angle_b',
# Return a stable position from a cartesian coordinate 'endstop_a', 'endstop_b', 'endstop_c')
def calc_stable_position(coord, delta_params): if is_extended:
dp = delta_params adj_params += ('arm_a', 'arm_b', 'arm_c')
steppos = [ return adj_params, self.params
math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2] def new_calibration(self, params):
for t, a in zip(dp.towers, dp.arms) ] return DeltaCalibration(params)
return [(ep - sp) / sd def save_state(self, configfile):
for sd, ep, sp in zip(dp.stepdists, dp.abs_endstops, steppos)] 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 # Load a stable position from a config entry
def load_config_stable(config, option): def load_config_stable(config, option):
@ -108,7 +126,7 @@ def measurements_to_distances(measured_params, delta_params):
outer_pos = [(ax * outer_ridge, ay * outer_ridge, 0.) outer_pos = [(ax * outer_ridge, ay * outer_ridge, 0.)
for ax, ay in xy_angles] for ax, ay in xy_angles]
center_positions = [ center_positions = [
(cd, calc_stable_position(ip, dp), calc_stable_position(op, dp)) (cd, dp.calc_stable_position(ip), dp.calc_stable_position(op))
for cd, ip, op in zip(center_dists, inner_pos, outer_pos)] for cd, ip, op in zip(center_dists, inner_pos, outer_pos)]
# Calculate positions of outer measurements # Calculate positions of outer measurements
outer_center = MeasureOuterRadius * scale outer_center = MeasureOuterRadius * scale
@ -119,7 +137,7 @@ def measurements_to_distances(measured_params, delta_params):
second_pos = [(ax * outer_ridge + spx, ay * outer_ridge + spy, 0.) second_pos = [(ax * outer_ridge + spx, ay * outer_ridge + spy, 0.)
for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)] for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
outer_positions = [ outer_positions = [
(od, calc_stable_position(fp, dp), calc_stable_position(sp, dp)) (od, dp.calc_stable_position(fp), dp.calc_stable_position(sp))
for od, fp, sp in zip(outer_dists, first_pos, second_pos)] for od, fp, sp in zip(outer_dists, first_pos, second_pos)]
return center_positions + outer_positions return center_positions + outer_positions
@ -168,17 +186,10 @@ class DeltaCalibrate:
desc=self.cmd_DELTA_CALIBRATE_help) desc=self.cmd_DELTA_CALIBRATE_help)
self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE, self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE,
desc=self.cmd_DELTA_ANALYZE_help) desc=self.cmd_DELTA_ANALYZE_help)
def save_state(self, probe_positions, distances, params): def save_state(self, probe_positions, distances, delta_params):
# Save main delta parameters # Save main delta parameters
configfile = self.printer.lookup_object('configfile') configfile = self.printer.lookup_object('configfile')
configfile.set('printer', 'delta_radius', "%.6f" % (params['radius'])) delta_params.save_state(configfile)
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],))
# Save probe stable positions # Save probe stable positions
section = 'delta_calibrate' section = 'delta_calibrate'
configfile.remove_section(section) configfile.remove_section(section)
@ -197,39 +208,36 @@ class DeltaCalibrate:
# Convert positions into (z_offset, stable_position) pairs # Convert positions into (z_offset, stable_position) pairs
z_offset = offsets[2] z_offset = offsets[2]
kin = self.printer.lookup_object('toolhead').get_kinematics() kin = self.printer.lookup_object('toolhead').get_kinematics()
delta_params = build_delta_params(kin.get_calibrate_params()) delta_params = DeltaCalibration(kin.get_calibrate_params())
probe_positions = [(z_offset, calc_stable_position(p, delta_params)) probe_positions = [(z_offset, delta_params.calc_stable_position(p))
for p in positions] for p in positions]
# Perform analysis # Perform analysis
self.calculate_params(probe_positions, self.last_distances) self.calculate_params(probe_positions, self.last_distances)
def calculate_params(self, probe_positions, distances): def calculate_params(self, probe_positions, distances):
# Setup for coordinate descent analysis # Setup for coordinate descent analysis
kin = self.printer.lookup_object('toolhead').get_kinematics() kin = self.printer.lookup_object('toolhead').get_kinematics()
params = kin.get_calibrate_params() orig_delta_params = odp = DeltaCalibration(kin.get_calibrate_params())
orig_delta_params = build_delta_params(params) adj_params, params = odp.coordinate_descent_params(distances)
logging.info("Calculating delta_calibrate with:\n%s\n%s\n" logging.info("Calculating delta_calibrate with:\n%s\n%s\n"
"Initial delta_calibrate parameters: %s", "Initial delta_calibrate parameters: %s",
probe_positions, distances, params) probe_positions, distances, params)
adj_params = ('radius', 'angle_a', 'angle_b',
'endstop_a', 'endstop_b', 'endstop_c')
z_weight = 1. z_weight = 1.
if distances: if distances:
adj_params += ('arm_a', 'arm_b', 'arm_c')
z_weight = len(distances) / (MEASURE_WEIGHT * len(probe_positions)) z_weight = len(distances) / (MEASURE_WEIGHT * len(probe_positions))
# Perform coordinate descent # Perform coordinate descent
def delta_errorfunc(params): def delta_errorfunc(params):
# Build new delta_params for params under test # Build new delta_params for params under test
delta_params = build_delta_params(params) delta_params = orig_delta_params.new_calibration(params)
# Calculate z height errors # Calculate z height errors
total_error = 0. total_error = 0.
for z_offset, stable_pos in probe_positions: for z_offset, stable_pos in probe_positions:
x, y, z = get_position_from_stable(stable_pos, delta_params) x, y, z = delta_params.get_position_from_stable(stable_pos)
total_error += (z - z_offset)**2 total_error += (z - z_offset)**2
total_error *= z_weight total_error *= z_weight
# Calculate distance errors # Calculate distance errors
for dist, stable_pos1, stable_pos2 in distances: for dist, stable_pos1, stable_pos2 in distances:
x1, y1, z1 = get_position_from_stable(stable_pos1, delta_params) x1, y1, z1 = delta_params.get_position_from_stable(stable_pos1)
x2, y2, z2 = get_position_from_stable(stable_pos2, delta_params) x2, y2, z2 = delta_params.get_position_from_stable(stable_pos2)
d = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2) d = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
total_error += (d - dist)**2 total_error += (d - dist)**2
return total_error return total_error
@ -237,37 +245,26 @@ class DeltaCalibrate:
self.printer, adj_params, params, delta_errorfunc) self.printer, adj_params, params, delta_errorfunc)
# Log and report results # Log and report results
logging.info("Calculated delta_calibrate parameters: %s", new_params) logging.info("Calculated delta_calibrate parameters: %s", new_params)
new_delta_params = build_delta_params(new_params) new_delta_params = orig_delta_params.new_calibration(new_params)
for z_offset, spos in probe_positions: for z_offset, spos in probe_positions:
logging.info("height orig: %.6f new: %.6f goal: %.6f", logging.info("height orig: %.6f new: %.6f goal: %.6f",
get_position_from_stable(spos, orig_delta_params)[2], orig_delta_params.get_position_from_stable(spos)[2],
get_position_from_stable(spos, new_delta_params)[2], new_delta_params.get_position_from_stable(spos)[2],
z_offset) z_offset)
for dist, spos1, spos2 in distances: for dist, spos1, spos2 in distances:
x1, y1, z1 = get_position_from_stable(spos1, orig_delta_params) x1, y1, z1 = orig_delta_params.get_position_from_stable(spos1)
x2, y2, z2 = get_position_from_stable(spos2, orig_delta_params) x2, y2, z2 = orig_delta_params.get_position_from_stable(spos2)
orig_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2) orig_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
x1, y1, z1 = get_position_from_stable(spos1, new_delta_params) x1, y1, z1 = new_delta_params.get_position_from_stable(spos1)
x2, y2, z2 = get_position_from_stable(spos2, new_delta_params) x2, y2, z2 = new_delta_params.get_position_from_stable(spos2)
new_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2) new_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
logging.info("distance orig: %.6f new: %.6f goal: %.6f", logging.info("distance orig: %.6f new: %.6f goal: %.6f",
orig_dist, new_dist, dist) orig_dist, new_dist, dist)
self.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"
"The SAVE_CONFIG command will update the printer config file\n"
"with these parameters and restart the printer." % (
new_params['endstop_a'], new_params['angle_a'],
new_params['arm_a'],
new_params['endstop_b'], new_params['angle_b'],
new_params['arm_b'],
new_params['endstop_c'], new_params['angle_c'],
new_params['arm_c'],
new_params['radius']))
# Store results for SAVE_CONFIG # Store results for SAVE_CONFIG
self.save_state(probe_positions, distances, new_params) self.save_state(probe_positions, distances, new_delta_params)
self.gcode.respond_info(
"The SAVE_CONFIG command will update the printer config file\n"
"with these parameters and restart the printer.")
cmd_DELTA_CALIBRATE_help = "Delta calibration script" cmd_DELTA_CALIBRATE_help = "Delta calibration script"
def cmd_DELTA_CALIBRATE(self, params): def cmd_DELTA_CALIBRATE(self, params):
self.probe_helper.start_probe(params) self.probe_helper.start_probe(params)
@ -279,7 +276,7 @@ class DeltaCalibrate:
raise self.gcode.error("Not all measurements provided") raise self.gcode.error("Not all measurements provided")
else: else:
kin = self.printer.lookup_object('toolhead').get_kinematics() kin = self.printer.lookup_object('toolhead').get_kinematics()
delta_params = build_delta_params(kin.get_calibrate_params()) delta_params = DeltaCalibration(kin.get_calibrate_params())
distances = measurements_to_distances( distances = measurements_to_distances(
self.delta_analyze_entry, delta_params) self.delta_analyze_entry, delta_params)
if not self.last_probe_positions: if not self.last_probe_positions: