delta: Support different arm lengths for each tower
Change the config file so that the delta arm length is specified per-tower. This makes it possible to support advanced calibration. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
efb4a5daa1
commit
bc5d900e61
|
@ -17,6 +17,11 @@ enable_pin: !ar38
|
|||
step_distance: .01
|
||||
endstop_pin: ^ar2
|
||||
position_endstop: 297.05
|
||||
arm_length: 333.0
|
||||
# Length (in mm) of the diagonal rod that connects this tower to the
|
||||
# print head. This parameter must be provided for stepper_a; for
|
||||
# stepper_b and stepper_c this parameter defaults to the value
|
||||
# specified for stepper_a.
|
||||
#angle:
|
||||
# This option specifies the angle (in degrees) that the tower is
|
||||
# at. The default is 210 for stepper_a, 330 for stepper_b, and 90
|
||||
|
@ -91,9 +96,6 @@ max_z_velocity: 150
|
|||
# maximum speed of up/down moves (which require a higher step rate
|
||||
# than other moves on a delta printer). The default is to use
|
||||
# max_velocity for max_z_velocity.
|
||||
delta_arm_length: 333.0
|
||||
# Length (in mm) of the diagonal rods that connect the linear axes
|
||||
# to the print head. This parameter must be provided.
|
||||
delta_radius: 174.75
|
||||
# Radius (in mm) of the horizontal circle formed by the three linear
|
||||
# axis towers. This parameter may also be calculated as:
|
||||
|
|
|
@ -13,17 +13,22 @@ SLOW_RATIO = 3.
|
|||
|
||||
class DeltaKinematics:
|
||||
def __init__(self, toolhead, printer, config):
|
||||
self.steppers = [stepper.PrinterHomingStepper(
|
||||
printer, config.getsection('stepper_' + n))
|
||||
for n in ['a', 'b', 'c']]
|
||||
stepper_configs = [config.getsection('stepper_' + n)
|
||||
for n in ['a', 'b', 'c']]
|
||||
self.steppers = [stepper.PrinterHomingStepper(printer, sconfig)
|
||||
for sconfig in stepper_configs]
|
||||
self.need_motor_enable = self.need_home = True
|
||||
radius = config.getfloat('delta_radius', above=0.)
|
||||
arm_length = config.getfloat('delta_arm_length', above=radius)
|
||||
self.arm_length2 = arm_length**2
|
||||
arm_length_a = stepper_configs[0].getfloat('arm_length', above=radius)
|
||||
arm_lengths = [sconfig.getfloat('arm_length', arm_length_a, above=radius)
|
||||
for sconfig in stepper_configs]
|
||||
self.arm2 = [arm**2 for arm in arm_lengths]
|
||||
self.endstops = [s.position_endstop + math.sqrt(arm2 - radius**2)
|
||||
for s, arm2 in zip(self.steppers, self.arm2)]
|
||||
self.limit_xy2 = -1.
|
||||
tower_height_at_zeros = math.sqrt(self.arm_length2 - radius**2)
|
||||
self.max_z = min([s.position_endstop for s in self.steppers])
|
||||
self.limit_z = self.max_z - (arm_length - tower_height_at_zeros)
|
||||
self.limit_z = min([ep - arm
|
||||
for ep, arm in zip(self.endstops, arm_lengths)])
|
||||
logging.info(
|
||||
"Delta max build height %.2fmm (radius tapered above %.2fmm)" % (
|
||||
self.max_z, self.limit_z))
|
||||
|
@ -36,22 +41,22 @@ class DeltaKinematics:
|
|||
for s in self.steppers:
|
||||
s.set_max_jerk(max_halt_velocity, self.max_accel)
|
||||
# Determine tower locations in cartesian space
|
||||
angles = [config.getsection('stepper_a').getfloat('angle', 210.),
|
||||
config.getsection('stepper_b').getfloat('angle', 330.),
|
||||
config.getsection('stepper_c').getfloat('angle', 90.)]
|
||||
angles = [sconfig.getfloat('angle', angle)
|
||||
for sconfig, angle in zip(stepper_configs, [210., 330., 90.])]
|
||||
self.towers = [(math.cos(math.radians(angle)) * radius,
|
||||
math.sin(math.radians(angle)) * radius)
|
||||
for angle in angles]
|
||||
# Find the point where an XY move could result in excessive
|
||||
# tower movement
|
||||
half_min_step_dist = min([s.step_dist for s in self.steppers]) * .5
|
||||
min_arm_length = min(arm_lengths)
|
||||
def ratio_to_dist(ratio):
|
||||
return (ratio * math.sqrt(self.arm_length2 / (ratio**2 + 1.)
|
||||
return (ratio * math.sqrt(min_arm_length**2 / (ratio**2 + 1.)
|
||||
- half_min_step_dist**2)
|
||||
+ half_min_step_dist)
|
||||
self.slow_xy2 = (ratio_to_dist(SLOW_RATIO) - radius)**2
|
||||
self.very_slow_xy2 = (ratio_to_dist(2. * SLOW_RATIO) - radius)**2
|
||||
self.max_xy2 = min(radius, arm_length - radius,
|
||||
self.max_xy2 = min(radius, min_arm_length - radius,
|
||||
ratio_to_dist(4. * SLOW_RATIO) - radius)**2
|
||||
logging.info(
|
||||
"Delta max build radius %.2fmm (moves slowed past %.2fmm and %.2fmm)"
|
||||
|
@ -61,8 +66,7 @@ class DeltaKinematics:
|
|||
def get_steppers(self):
|
||||
return list(self.steppers)
|
||||
def _cartesian_to_actuator(self, coord):
|
||||
return [math.sqrt(self.arm_length2
|
||||
- (self.towers[i][0] - coord[0])**2
|
||||
return [math.sqrt(self.arm2[i] - (self.towers[i][0] - coord[0])**2
|
||||
- (self.towers[i][1] - coord[1])**2) + coord[2]
|
||||
for i in StepList]
|
||||
def _actuator_to_cartesian(self, pos):
|
||||
|
@ -82,9 +86,9 @@ class DeltaKinematics:
|
|||
ez = matrix_cross(ex, ey)
|
||||
j = matrix_dot(ey, s31)
|
||||
|
||||
x = d**2 / (2. * d)
|
||||
y = (j**2 + (x-i)**2 - x**2) / (2. * j)
|
||||
z = -math.sqrt(self.arm_length2 - x**2 - y**2)
|
||||
x = (self.arm2[0] - self.arm2[1] + d**2) / (2. * d)
|
||||
y = (self.arm2[0] - self.arm2[2] - x**2 + (x-i)**2 + j**2) / (2. * j)
|
||||
z = -math.sqrt(self.arm2[0] - x**2 - y**2)
|
||||
|
||||
ex_x = matrix_mul(ex, x)
|
||||
ey_y = matrix_mul(ey, y)
|
||||
|
@ -105,7 +109,7 @@ class DeltaKinematics:
|
|||
homing_speed = s.get_homing_speed()
|
||||
homepos = [0., 0., self.max_z, None]
|
||||
coord = list(homepos)
|
||||
coord[2] = -1.5 * math.sqrt(self.arm_length2-self.max_xy2)
|
||||
coord[2] = -1.5 * math.sqrt(max(self.arm2)-self.max_xy2)
|
||||
homing_state.home(coord, homepos, endstops, homing_speed)
|
||||
# Retract
|
||||
coord[2] = homepos[2] - s.homing_retract_dist
|
||||
|
@ -115,10 +119,8 @@ class DeltaKinematics:
|
|||
homing_state.home(coord, homepos, endstops,
|
||||
homing_speed/2.0, second_home=True)
|
||||
# Set final homed position
|
||||
spos = self._cartesian_to_actuator(homepos)
|
||||
spos = [spos[i] + self.steppers[i].position_endstop - self.max_z
|
||||
+ self.steppers[i].get_homed_offset()
|
||||
for i in StepList]
|
||||
spos = [ep + s.get_homed_offset()
|
||||
for ep, s in zip(self.endstops, self.steppers)]
|
||||
homing_state.set_homed_position(self._actuator_to_cartesian(spos))
|
||||
def motor_off(self, print_time):
|
||||
self.limit_xy2 = -1.
|
||||
|
@ -192,7 +194,7 @@ class DeltaKinematics:
|
|||
towery_d = self.towers[i][1] - origy
|
||||
vt_startxy_d = (towerx_d*axes_d[0] + towery_d*axes_d[1])*inv_movexy_d
|
||||
tangentxy_d2 = towerx_d**2 + towery_d**2 - vt_startxy_d**2
|
||||
vt_arm_d = math.sqrt(self.arm_length2 - tangentxy_d2)
|
||||
vt_arm_d = math.sqrt(self.arm2[i] - tangentxy_d2)
|
||||
vt_startz = origz
|
||||
|
||||
# Generate steps
|
||||
|
|
Loading…
Reference in New Issue