diff --git a/config/example-delta.cfg b/config/example-delta.cfg index ca11ac1b..dbd6f191 100644 --- a/config/example-delta.cfg +++ b/config/example-delta.cfg @@ -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: diff --git a/klippy/delta.py b/klippy/delta.py index 0ab914cf..5900a183 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -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