mirror of https://github.com/Desuuuu/klipper.git
delta_calibrate: Initial support for enhanced delta calibration
Add support for an enhanced delta calibration routine that performs XY dimension calibration. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
929733f0a7
commit
0b2c89ecaf
|
@ -0,0 +1,77 @@
|
||||||
|
// Calibration object for delta sizing
|
||||||
|
//
|
||||||
|
// Generate STL using OpenSCAD:
|
||||||
|
// openscad calibrate_size.scad -o calibrate_size.stl
|
||||||
|
|
||||||
|
base_radius = 70;
|
||||||
|
base_height = 1.5;
|
||||||
|
base_width = 8;
|
||||||
|
cylinder_height = 5;
|
||||||
|
cylinder_radius = 5;
|
||||||
|
cylinder_outer_dist = 65;
|
||||||
|
ridge_cut_radius = .5;
|
||||||
|
text_height = 1;
|
||||||
|
text_size = 5;
|
||||||
|
spoke_angles = [0, 60, 120, 180, 240, 300];
|
||||||
|
CUT=0.01;
|
||||||
|
|
||||||
|
// Circular ring around entire object (to help reduce warping)
|
||||||
|
module base_ring() {
|
||||||
|
difference() {
|
||||||
|
cylinder(h=base_height, r=base_radius);
|
||||||
|
translate([0, 0, -CUT])
|
||||||
|
cylinder(h=base_height + 2*CUT, r=base_radius-base_width);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// The base ring plus the base spokes
|
||||||
|
module base() {
|
||||||
|
base_ring();
|
||||||
|
// Spokes
|
||||||
|
for (angle=spoke_angles)
|
||||||
|
rotate([0, 0, angle])
|
||||||
|
translate([-base_width/2, -CUT, 0])
|
||||||
|
cube([base_width, base_radius-base_width+2*CUT, base_height]);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Cylinder that measurement ridges are cut out of
|
||||||
|
module measuring_cylinder() {
|
||||||
|
cut_width = cylinder_radius;
|
||||||
|
difference() {
|
||||||
|
cylinder(h=cylinder_height+CUT, r=cylinder_radius, $fn=60);
|
||||||
|
for (angle=spoke_angles)
|
||||||
|
rotate([0, 0, angle])
|
||||||
|
translate([-cut_width, cylinder_radius - ridge_cut_radius, -CUT])
|
||||||
|
cube([2*cut_width, cut_width, cylinder_height+3*CUT]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// All the measuring cylinders around the ring
|
||||||
|
module measuring_cylinders() {
|
||||||
|
measuring_cylinder();
|
||||||
|
for (angle=spoke_angles)
|
||||||
|
rotate([0, 0, angle])
|
||||||
|
translate([0, cylinder_outer_dist, 0])
|
||||||
|
measuring_cylinder();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Text writing
|
||||||
|
module write_text(angle, dist, msg) {
|
||||||
|
text_offset = dist + 1 - text_size/2;
|
||||||
|
rotate([0, 0, angle])
|
||||||
|
translate([0, text_offset, base_height - CUT])
|
||||||
|
linear_extrude(height=text_height + CUT)
|
||||||
|
text(msg, size=text_size, halign="center");
|
||||||
|
}
|
||||||
|
|
||||||
|
// Final object with text descriptions
|
||||||
|
module calibration_object() {
|
||||||
|
base();
|
||||||
|
translate([0, 0, base_height-CUT])
|
||||||
|
measuring_cylinders();
|
||||||
|
write_text(120, cylinder_outer_dist - 20, "A");
|
||||||
|
write_text(240, cylinder_outer_dist - 20, "B");
|
||||||
|
write_text(0, cylinder_outer_dist - 20, "C");
|
||||||
|
}
|
||||||
|
|
||||||
|
calibration_object();
|
File diff suppressed because it is too large
Load Diff
|
@ -56,6 +56,15 @@ def get_stable_position(stepper_position, delta_params):
|
||||||
for sd, ep, sp in zip(
|
for sd, ep, sp in zip(
|
||||||
dp.stepdists, dp.abs_endstops, stepper_position)]
|
dp.stepdists, dp.abs_endstops, stepper_position)]
|
||||||
|
|
||||||
|
# Return a stable position from a cartesian coordinate
|
||||||
|
def calc_stable_position(coord, delta_params):
|
||||||
|
dp = delta_params
|
||||||
|
steppos = [
|
||||||
|
math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2]
|
||||||
|
for t, a in zip(dp.towers, dp.arms) ]
|
||||||
|
return [(ep - sp) / sd
|
||||||
|
for sd, ep, sp in zip(dp.stepdists, dp.abs_endstops, steppos)]
|
||||||
|
|
||||||
# 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):
|
||||||
spos = config.get(option)
|
spos = config.get(option)
|
||||||
|
@ -68,6 +77,60 @@ def load_config_stable(config, option):
|
||||||
return sa, sb, sc
|
return sa, sb, sc
|
||||||
|
|
||||||
|
|
||||||
|
######################################################################
|
||||||
|
# Delta calibration object
|
||||||
|
######################################################################
|
||||||
|
|
||||||
|
# The angles and distances of the calibration object found in
|
||||||
|
# docs/prints/calibrate_size.stl
|
||||||
|
MeasureAngles = [210., 270., 330., 30., 90., 150.]
|
||||||
|
MeasureOuterRadius = 65
|
||||||
|
MeasureRidgeRadius = 5. - .5
|
||||||
|
|
||||||
|
# How much to prefer a distance measurement over a height measurement
|
||||||
|
MEASURE_WEIGHT = 0.5
|
||||||
|
|
||||||
|
# Convert distance measurements made on the calibration object to
|
||||||
|
# 3-tuples of (actual_distance, stable_position1, stable_position2)
|
||||||
|
def measurements_to_distances(measured_params, delta_params):
|
||||||
|
# Extract params
|
||||||
|
mp = measured_params
|
||||||
|
dp = delta_params
|
||||||
|
scale = mp['SCALE'][0]
|
||||||
|
cpw = mp['CENTER_PILLAR_WIDTHS']
|
||||||
|
center_widths = [cpw[0], cpw[2], cpw[1], cpw[0], cpw[2], cpw[1]]
|
||||||
|
center_dists = [od - cw
|
||||||
|
for od, cw in zip(mp['CENTER_DISTS'], center_widths)]
|
||||||
|
outer_dists = [
|
||||||
|
od - opw
|
||||||
|
for od, opw in zip(mp['OUTER_DISTS'], mp['OUTER_PILLAR_WIDTHS']) ]
|
||||||
|
# Convert angles in degrees to an XY multiplier
|
||||||
|
obj_angles = map(math.radians, MeasureAngles)
|
||||||
|
xy_angles = zip(map(math.cos, obj_angles), map(math.sin, obj_angles))
|
||||||
|
# Calculate stable positions for center measurements
|
||||||
|
inner_ridge = MeasureRidgeRadius * scale
|
||||||
|
inner_pos = [(ax * inner_ridge, ay * inner_ridge, 0.)
|
||||||
|
for ax, ay in xy_angles]
|
||||||
|
outer_ridge = (MeasureOuterRadius + MeasureRidgeRadius) * scale
|
||||||
|
outer_pos = [(ax * outer_ridge, ay * outer_ridge, 0.)
|
||||||
|
for ax, ay in xy_angles]
|
||||||
|
center_positions = [
|
||||||
|
(cd, calc_stable_position(ip, dp), calc_stable_position(op, dp))
|
||||||
|
for cd, ip, op in zip(center_dists, inner_pos, outer_pos)]
|
||||||
|
# Calculate positions of outer measurements
|
||||||
|
outer_center = MeasureOuterRadius * scale
|
||||||
|
start_pos = [(ax * outer_center, ay * outer_center) for ax, ay in xy_angles]
|
||||||
|
shifted_angles = xy_angles[2:] + xy_angles[:2]
|
||||||
|
first_pos = [(ax * inner_ridge + spx, ay * inner_ridge + spy, 0.)
|
||||||
|
for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
|
||||||
|
second_pos = [(ax * outer_ridge + spx, ay * outer_ridge + spy, 0.)
|
||||||
|
for (ax, ay), (spx, spy) in zip(shifted_angles, start_pos)]
|
||||||
|
outer_positions = [
|
||||||
|
(od, calc_stable_position(fp, dp), calc_stable_position(sp, dp))
|
||||||
|
for od, fp, sp in zip(outer_dists, first_pos, second_pos)]
|
||||||
|
return center_positions + outer_positions
|
||||||
|
|
||||||
|
|
||||||
######################################################################
|
######################################################################
|
||||||
# Delta Calibrate class
|
# Delta Calibrate class
|
||||||
######################################################################
|
######################################################################
|
||||||
|
@ -95,12 +158,23 @@ class DeltaCalibrate:
|
||||||
break
|
break
|
||||||
height_pos = load_config_stable(config, "height%d_pos" % (i,))
|
height_pos = load_config_stable(config, "height%d_pos" % (i,))
|
||||||
self.last_probe_positions.append((height, height_pos))
|
self.last_probe_positions.append((height, height_pos))
|
||||||
# Register DELTA_CALIBRATE command
|
# Restore distance measurements
|
||||||
|
self.delta_analyze_entry = {'SCALE': (1.,)}
|
||||||
|
self.last_distances = []
|
||||||
|
for i in range(999):
|
||||||
|
dist = config.getfloat("distance%d" % (i,), None)
|
||||||
|
if dist is None:
|
||||||
|
break
|
||||||
|
distance_pos1 = load_config_stable(config, "distance%d_pos1" % (i,))
|
||||||
|
distance_pos2 = load_config_stable(config, "distance%d_pos2" % (i,))
|
||||||
|
self.last_distances.append((dist, distance_pos1, distance_pos2))
|
||||||
|
# Register gcode commands
|
||||||
self.gcode = self.printer.lookup_object('gcode')
|
self.gcode = self.printer.lookup_object('gcode')
|
||||||
self.gcode.register_command(
|
self.gcode.register_command('DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
|
||||||
'DELTA_CALIBRATE', self.cmd_DELTA_CALIBRATE,
|
|
||||||
desc=self.cmd_DELTA_CALIBRATE_help)
|
desc=self.cmd_DELTA_CALIBRATE_help)
|
||||||
def save_state(self, probe_positions, params):
|
self.gcode.register_command('DELTA_ANALYZE', self.cmd_DELTA_ANALYZE,
|
||||||
|
desc=self.cmd_DELTA_ANALYZE_help)
|
||||||
|
def save_state(self, probe_positions, distances, 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']))
|
configfile.set('printer', 'delta_radius', "%.6f" % (params['radius']))
|
||||||
|
@ -118,10 +192,13 @@ class DeltaCalibrate:
|
||||||
configfile.set(section, "height%d" % (i,), z_offset)
|
configfile.set(section, "height%d" % (i,), z_offset)
|
||||||
configfile.set(section, "height%d_pos" % (i,),
|
configfile.set(section, "height%d_pos" % (i,),
|
||||||
"%d,%d,%d" % tuple(spos))
|
"%d,%d,%d" % tuple(spos))
|
||||||
cmd_DELTA_CALIBRATE_help = "Delta calibration script"
|
# Save distance measurements
|
||||||
def cmd_DELTA_CALIBRATE(self, params):
|
for i, (dist, spos1, spos2) in enumerate(distances):
|
||||||
self.gcode.run_script_from_command("G28")
|
configfile.set(section, "distance%d" % (i,), dist)
|
||||||
self.probe_helper.start_probe()
|
configfile.set(section, "distance%d_pos1" % (i,),
|
||||||
|
"%.3f,%.3f,%.3f" % tuple(spos1))
|
||||||
|
configfile.set(section, "distance%d_pos2" % (i,),
|
||||||
|
"%.3f,%.3f,%.3f" % tuple(spos2))
|
||||||
def get_probed_position(self):
|
def get_probed_position(self):
|
||||||
kin = self.printer.lookup_object('toolhead').get_kinematics()
|
kin = self.printer.lookup_object('toolhead').get_kinematics()
|
||||||
return [s.get_commanded_position() for s in kin.get_steppers()]
|
return [s.get_commanded_position() for s in kin.get_steppers()]
|
||||||
|
@ -133,24 +210,42 @@ class DeltaCalibrate:
|
||||||
probe_positions = [(z_offset, get_stable_position(p, delta_params))
|
probe_positions = [(z_offset, get_stable_position(p, delta_params))
|
||||||
for p in positions]
|
for p in positions]
|
||||||
# Perform analysis
|
# Perform analysis
|
||||||
self.calculate_params(probe_positions)
|
self.calculate_params(probe_positions, self.last_distances)
|
||||||
def calculate_params(self, probe_positions):
|
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()
|
params = kin.get_calibrate_params()
|
||||||
orig_delta_params = build_delta_params(params)
|
orig_delta_params = build_delta_params(params)
|
||||||
logging.info("Calculating delta_calibrate with: %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, params)
|
probe_positions, distances, params)
|
||||||
adj_params = ('radius', 'angle_a', 'angle_b',
|
adj_params = ('radius', 'angle_a', 'angle_b',
|
||||||
'endstop_a', 'endstop_b', 'endstop_c')
|
'endstop_a', 'endstop_b', 'endstop_c')
|
||||||
|
z_weight = 1.
|
||||||
|
if distances:
|
||||||
|
adj_params += ('arm_a', 'arm_b', 'arm_c')
|
||||||
|
z_weight = len(distances) / (MEASURE_WEIGHT * len(probe_positions))
|
||||||
# Perform coordinate descent
|
# Perform coordinate descent
|
||||||
|
call_count = [0]
|
||||||
def delta_errorfunc(params):
|
def delta_errorfunc(params):
|
||||||
|
call_count[0] += 1
|
||||||
|
if not call_count[0] % 1000:
|
||||||
|
self.gcode.respond_info("Working on calibration...")
|
||||||
|
self.printer.get_reactor().pause(0.)
|
||||||
|
# Build new delta_params for params under test
|
||||||
delta_params = build_delta_params(params)
|
delta_params = build_delta_params(params)
|
||||||
|
# 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 = get_position_from_stable(stable_pos, delta_params)
|
||||||
total_error += (z - z_offset)**2
|
total_error += (z - z_offset)**2
|
||||||
|
total_error *= z_weight
|
||||||
|
# Calculate distance errors
|
||||||
|
for dist, stable_pos1, stable_pos2 in distances:
|
||||||
|
x1, y1, z1 = get_position_from_stable(stable_pos1, delta_params)
|
||||||
|
x2, y2, z2 = get_position_from_stable(stable_pos2, delta_params)
|
||||||
|
d = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
|
||||||
|
total_error += (d - dist)**2
|
||||||
return total_error
|
return total_error
|
||||||
new_params = mathutil.coordinate_descent(
|
new_params = mathutil.coordinate_descent(
|
||||||
adj_params, params, delta_errorfunc)
|
adj_params, params, delta_errorfunc)
|
||||||
|
@ -162,19 +257,77 @@ class DeltaCalibrate:
|
||||||
get_position_from_stable(spos, orig_delta_params)[2],
|
get_position_from_stable(spos, orig_delta_params)[2],
|
||||||
get_position_from_stable(spos, new_delta_params)[2],
|
get_position_from_stable(spos, new_delta_params)[2],
|
||||||
z_offset)
|
z_offset)
|
||||||
|
for dist, spos1, spos2 in distances:
|
||||||
|
x1, y1, z1 = get_position_from_stable(spos1, orig_delta_params)
|
||||||
|
x2, y2, z2 = get_position_from_stable(spos2, orig_delta_params)
|
||||||
|
orig_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
|
||||||
|
x1, y1, z1 = get_position_from_stable(spos1, new_delta_params)
|
||||||
|
x2, y2, z2 = get_position_from_stable(spos2, new_delta_params)
|
||||||
|
new_dist = math.sqrt((x1-x2)**2 + (y1-y2)**2 + (z1-z2)**2)
|
||||||
|
logging.info("distance orig: %.6f new: %.6f goal: %.6f",
|
||||||
|
orig_dist, new_dist, dist)
|
||||||
self.gcode.respond_info(
|
self.gcode.respond_info(
|
||||||
"stepper_a: position_endstop: %.6f angle: %.6f\n"
|
"stepper_a: position_endstop: %.6f angle: %.6f arm: %.6f\n"
|
||||||
"stepper_b: position_endstop: %.6f angle: %.6f\n"
|
"stepper_b: position_endstop: %.6f angle: %.6f arm: %.6f\n"
|
||||||
"stepper_c: position_endstop: %.6f angle: %.6f\n"
|
"stepper_c: position_endstop: %.6f angle: %.6f arm: %.6f\n"
|
||||||
"delta_radius: %.6f\n"
|
"delta_radius: %.6f\n"
|
||||||
"The SAVE_CONFIG command will update the printer config file\n"
|
"The SAVE_CONFIG command will update the printer config file\n"
|
||||||
"with these parameters and restart the printer." % (
|
"with these parameters and restart the printer." % (
|
||||||
new_params['endstop_a'], new_params['angle_a'],
|
new_params['endstop_a'], new_params['angle_a'],
|
||||||
|
new_params['arm_a'],
|
||||||
new_params['endstop_b'], new_params['angle_b'],
|
new_params['endstop_b'], new_params['angle_b'],
|
||||||
|
new_params['arm_b'],
|
||||||
new_params['endstop_c'], new_params['angle_c'],
|
new_params['endstop_c'], new_params['angle_c'],
|
||||||
|
new_params['arm_c'],
|
||||||
new_params['radius']))
|
new_params['radius']))
|
||||||
# Store results for SAVE_CONFIG
|
# Store results for SAVE_CONFIG
|
||||||
self.save_state(probe_positions, new_params)
|
self.save_state(probe_positions, distances, new_params)
|
||||||
|
cmd_DELTA_CALIBRATE_help = "Delta calibration script"
|
||||||
|
def cmd_DELTA_CALIBRATE(self, params):
|
||||||
|
self.gcode.run_script_from_command("G28")
|
||||||
|
self.probe_helper.start_probe()
|
||||||
|
def do_extended_calibration(self):
|
||||||
|
# Extract distance positions
|
||||||
|
if len(self.delta_analyze_entry) <= 1:
|
||||||
|
distances = self.last_distances
|
||||||
|
elif len(self.delta_analyze_entry) < 5:
|
||||||
|
raise self.gcode.error("Not all measurements provided")
|
||||||
|
else:
|
||||||
|
kin = self.printer.lookup_object('toolhead').get_kinematics()
|
||||||
|
delta_params = build_delta_params(kin.get_calibrate_params())
|
||||||
|
distances = measurements_to_distances(
|
||||||
|
self.delta_analyze_entry, delta_params)
|
||||||
|
if not self.last_probe_positions:
|
||||||
|
raise self.gcode.error(
|
||||||
|
"Must run basic calibration with DELTA_CALIBRATE first")
|
||||||
|
# Perform analysis
|
||||||
|
self.calculate_params(self.last_probe_positions, distances)
|
||||||
|
cmd_DELTA_ANALYZE_help = "Extended delta calibration tool"
|
||||||
|
def cmd_DELTA_ANALYZE(self, params):
|
||||||
|
# Parse distance measurements
|
||||||
|
args = {'CENTER_DISTS': 6, 'CENTER_PILLAR_WIDTHS': 3,
|
||||||
|
'OUTER_DISTS': 6, 'OUTER_PILLAR_WIDTHS': 6, 'SCALE': 1}
|
||||||
|
for name, count in args.items():
|
||||||
|
if name not in params:
|
||||||
|
continue
|
||||||
|
data = self.gcode.get_str(name, params)
|
||||||
|
try:
|
||||||
|
parts = map(float, data.split(','))
|
||||||
|
except:
|
||||||
|
raise self.gcode.error("Unable to parse parameter '%s'" % (
|
||||||
|
name,))
|
||||||
|
if len(parts) != count:
|
||||||
|
raise self.gcode.error("Parameter '%s' must have %d values" % (
|
||||||
|
name, count))
|
||||||
|
self.delta_analyze_entry[name] = parts
|
||||||
|
logging.info("DELTA_ANALYZE %s = %s", name, parts)
|
||||||
|
# Perform analysis if requested
|
||||||
|
if 'CALIBRATE' in params:
|
||||||
|
action = self.gcode.get_str('CALIBRATE', params)
|
||||||
|
actions = {'extended': 1}
|
||||||
|
if action not in actions:
|
||||||
|
raise self.gcode.error("Unknown calibrate action")
|
||||||
|
self.do_extended_calibration()
|
||||||
|
|
||||||
def load_config(config):
|
def load_config(config):
|
||||||
return DeltaCalibrate(config)
|
return DeltaCalibrate(config)
|
||||||
|
|
Loading…
Reference in New Issue