quad_gantry_level: Add gantry leveling (ruled hyperbolic parabola correction) (#631)
Signed-off-by: Maks Zolin <mzolin@vorondesign.com>
This commit is contained in:
Maks Zolin
KevinOConnor
parent
0c0087ce42
commit
e3a49dc692
|
|
@ -933,3 +933,43 @@
|
|||
# Replicape support - see the generic-replicape.cfg file for further
|
||||
# details.
|
||||
#[replicape]
|
||||
|
||||
# Moving gantry leveling using 4 independently controlled Z motors.
|
||||
# Corrects hyperbolic parabola effects (potato chip) on moving gantry
|
||||
# which is more flexible.
|
||||
# WARNING: Using this on a moving bed may lead to undesirable results.
|
||||
# If this section is present then a QUAD_GANTRY_LEVEL extended G-Code
|
||||
# command becomes available. This routine assumes the following Z motor
|
||||
# configuration:
|
||||
# ----------------
|
||||
# |Z1 Z2|
|
||||
# | --------- |
|
||||
# | | | |
|
||||
# | | | |
|
||||
# | x-------- |
|
||||
# |Z Z3|
|
||||
# ----------------
|
||||
# Where x is the (0,0) point on the bed
|
||||
#[quad_gantry_level]
|
||||
#gantry_corners:
|
||||
# A newline separated list of X,Y coordinates describing the
|
||||
# two opposing corners of the gantry. The first entry corresponds to
|
||||
# Z, the second to Z2.
|
||||
# This parameter must be provided.
|
||||
#points:
|
||||
# A newline separated list of four X,Y points that should be probed
|
||||
# during a QUAD_GANTRY_LEVEL command.
|
||||
# Order of the locations is important, and should correspond to Z,Z1
|
||||
# Z2, and Z3 location in order.
|
||||
# This parameter must be provided.
|
||||
# For maximum accuracy, ensure your probe offsets are configured.
|
||||
#speed: 50
|
||||
# The speed (in mm/s) of non-probing moves during the calibration.
|
||||
# The default is 50.
|
||||
#horizontal_move_z: 5
|
||||
# The height (in mm) that the head should be commanded to move to
|
||||
# just prior to starting a probe operation. The default is 5
|
||||
#samples: 1
|
||||
# Number of probe samples per point. The defaut is 1
|
||||
#sample_retract_dist: 2.0
|
||||
# Distance in mm to retract the probe between samples. Default is 2.
|
||||
|
|
|
|||
|
|
@ -0,0 +1,124 @@
|
|||
# Mechanicaly conforms a moving gantry to the bed with 4 Z steppers
|
||||
#
|
||||
# Copyright (C) 2018 Maks Zolin <mzolin@mzbot.us>
|
||||
#
|
||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||
import logging
|
||||
import probe
|
||||
|
||||
class QuadGantryLevel:
|
||||
def __init__(self, config):
|
||||
self.printer = config.get_printer()
|
||||
self.probe_helper = probe.ProbePointsHelper(config, self)
|
||||
gantry_corners = config.get('gantry_corners').split('\n')
|
||||
try:
|
||||
gantry_corners = [line.split(',', 1)
|
||||
for line in gantry_corners if line.strip()]
|
||||
self.gantry_corners = [(float(zp[0].strip()), float(zp[1].strip()))
|
||||
for zp in gantry_corners]
|
||||
except:
|
||||
raise config.error("Unable to parse gantry_corners in %s" % (
|
||||
config.get_name()))
|
||||
if len(self.gantry_corners) < 2:
|
||||
raise config.error("quad_gantry_level requires at least two gantry_corners")
|
||||
self.z_steppers = []
|
||||
# Register QUAD_GANTRY_LEVEL command
|
||||
self.gcode = self.printer.lookup_object('gcode')
|
||||
self.gcode.register_command(
|
||||
'QUAD_GANTRY_LEVEL', self.cmd_QUAD_GANTRY_LEVEL,
|
||||
desc=self.cmd_QUAD_GANTRY_LEVEL_help)
|
||||
def printer_state(self, state):
|
||||
if state == 'connect':
|
||||
self.handle_connect()
|
||||
def handle_connect(self):
|
||||
kin = self.printer.lookup_object('toolhead').get_kinematics()
|
||||
z_steppers = kin.get_steppers('Z')
|
||||
if len(z_steppers) != 4:
|
||||
raise self.printer.config_error("quad_gantry_level needs exactly 4 z steppers")
|
||||
self.z_steppers = z_steppers
|
||||
cmd_QUAD_GANTRY_LEVEL_help = "Conform a moving, twistable gantry to the shape of a stationary bed"
|
||||
def cmd_QUAD_GANTRY_LEVEL(self, params):
|
||||
self.probe_helper.start_probe()
|
||||
def get_probed_position(self):
|
||||
kin = self.printer.lookup_object('toolhead').get_kinematics()
|
||||
return kin.calc_position()
|
||||
def squash_positions(self,positions):
|
||||
# Group multi-probe data and average out the Z readings
|
||||
# Assumes samples come in sequentially
|
||||
grouped_pos = []
|
||||
for position in positions:
|
||||
if len(grouped_pos) > 0 and grouped_pos[-1][0] == position[0] and grouped_pos[-1][1] == position[1]:
|
||||
grouped_pos[-1][2].append(position[2])
|
||||
else:
|
||||
grouped_pos.append(position)
|
||||
grouped_pos[-1][2] = [grouped_pos[-1][2]]
|
||||
for id,pos in enumerate(grouped_pos):
|
||||
grouped_pos[id][2] = sum(grouped_pos[id][2]) / len(grouped_pos[id][2])
|
||||
return grouped_pos
|
||||
def finalize(self, offsets, positions):
|
||||
if len(positions) > 4:
|
||||
positions = self.squash_positions(positions)
|
||||
logging.info("quad_gantry_level Calculating gantry geometry with: %s", positions)
|
||||
p1 = [positions[0][0] + offsets[0],positions[0][2]]
|
||||
p2 = [positions[1][0] + offsets[0],positions[1][2]]
|
||||
p3 = [positions[2][0] + offsets[0],positions[2][2]]
|
||||
p4 = [positions[3][0] + offsets[0],positions[3][2]]
|
||||
f1 = self.linefit(p1,p4)
|
||||
f2 = self.linefit(p2,p3)
|
||||
logging.info("quad_gantry_level f1: %s, f2: %s" % (f1,f2))
|
||||
a1 = [positions[0][1] + offsets[1], self.plot(f1,self.gantry_corners[0][0])]
|
||||
a2 = [positions[1][1] + offsets[1], self.plot(f2,self.gantry_corners[0][0])]
|
||||
b1 = [positions[0][1] + offsets[1], self.plot(f1,self.gantry_corners[1][0])]
|
||||
b2 = [positions[1][1] + offsets[1], self.plot(f2,self.gantry_corners[1][0])]
|
||||
logging.info("quad_gantry_level a1: %s a2: %s b1: %s b2: %s\n" % (a1,a2,b1,b2))
|
||||
af = self.linefit(a1,a2)
|
||||
bf = self.linefit(b1,b2)
|
||||
logging.info("quad_gantry_level af: %s, bf: %s" % (af,bf))
|
||||
z_adjust = [0,0,0,0]
|
||||
z_adjust[0] = self.plot(af,self.gantry_corners[0][1])
|
||||
z_adjust[1] = self.plot(af,self.gantry_corners[1][1])
|
||||
z_adjust[2] = self.plot(bf,self.gantry_corners[1][1])
|
||||
z_adjust[3] = self.plot(bf,self.gantry_corners[0][1])
|
||||
z_avg = sum(z_adjust)/len(z_adjust)
|
||||
for z_id in range(4):
|
||||
z_adjust[z_id] = z_avg - z_adjust[z_id]
|
||||
try:
|
||||
self.adjust_steppers(z_adjust)
|
||||
except:
|
||||
logging.exception("quad_gantry_level adjust_steppers")
|
||||
for s in self.z_steppers:
|
||||
z.set_ignore_move(False)
|
||||
raise
|
||||
def linefit(self,p1,p2):
|
||||
if p1[1] == p2[1]:
|
||||
# Straight line
|
||||
return 0,p1[1]
|
||||
m = (p2[1] - p1[1])/(p2[0] - p1[0])
|
||||
b = p1[1] - m * p1[0]
|
||||
return m,b
|
||||
def plot(self,f,x):
|
||||
return f[0]*x + f[1]
|
||||
def adjust_steppers(self, z_adjust):
|
||||
msg = "Making the following gantry adjustments:\n%s\n" % (
|
||||
"\n".join(["%s = %.6f" % (
|
||||
self.z_steppers[z_id].get_name(), z_adjust[z_id]
|
||||
) for z_id in range(4)]))
|
||||
self.gcode.respond_info(msg)
|
||||
toolhead = self.printer.lookup_object('toolhead')
|
||||
cur_pos = toolhead.get_position()
|
||||
speed = self.probe_helper.get_lift_speed() / 2
|
||||
for s in self.z_steppers:
|
||||
s.set_ignore_move(True)
|
||||
for z_id in range(len(z_adjust)):
|
||||
stepper = self.z_steppers[z_id]
|
||||
stepper.set_ignore_move(False)
|
||||
cur_pos[2] = cur_pos[2] - z_adjust[z_id]
|
||||
toolhead.move(cur_pos, speed)
|
||||
toolhead.set_position(cur_pos)
|
||||
stepper.set_ignore_move(True)
|
||||
for s in self.z_steppers:
|
||||
s.set_ignore_move(False)
|
||||
self.gcode.reset_last_position()
|
||||
|
||||
def load_config(config):
|
||||
return QuadGantryLevel(config)
|
||||
Loading…
Reference in New Issue