QIDITECH/klipper
1
0
Fork 0
mirror of https://github.com/QIDITECH/klipper.git synced 2026-01-30 23:48:43 +03:00
Code Issues Packages Projects Releases Wiki Activity

klipper update

Browse Source
This commit is contained in:
Rainboooom
2023-06-15 11:41:08 +08:00
parent 845d13acb1
commit dffff1ae35
1921 changed files with 1625400 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
klippy/kinematics/__init__.py Normal file
View File

@@ -0,0 +1,5 @@
# Package definition for the kinematics directory
#
# Copyright (C) 2018 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.

BIN
klippy/kinematics/__init__.pyc Normal file
View File

Binary file not shown.

142
klippy/kinematics/cartesian.py Normal file
View File

@@ -0,0 +1,142 @@
# Code for handling the kinematics of cartesian robots
#
# Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging
import stepper
class CartKinematics:
def __init__(self, toolhead, config):
self.printer = config.get_printer()
# Setup axis rails
self.dual_carriage_axis = None
self.dual_carriage_rails = []
self.rails = [stepper.LookupMultiRail(config.getsection('stepper_' + n))
for n in 'xyz']
for rail, axis in zip(self.rails, 'xyz'):
rail.setup_itersolve('cartesian_stepper_alloc', axis.encode())
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
self.printer.register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat('max_z_velocity', max_velocity,
above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat('max_z_accel', max_accel,
above=0., maxval=max_accel)
self.limits = [(1.0, -1.0)] * 3
ranges = [r.get_range() for r in self.rails]
self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
# Check for dual carriage support
if config.has_section('dual_carriage'):
dc_config = config.getsection('dual_carriage')
dc_axis = dc_config.getchoice('axis', {'x': 'x', 'y': 'y'})
self.dual_carriage_axis = {'x': 0, 'y': 1}[dc_axis]
dc_rail = stepper.LookupMultiRail(dc_config)
dc_rail.setup_itersolve('cartesian_stepper_alloc', dc_axis.encode())
for s in dc_rail.get_steppers():
toolhead.register_step_generator(s.generate_steps)
self.dual_carriage_rails = [
self.rails[self.dual_carriage_axis], dc_rail]
self.printer.lookup_object('gcode').register_command(
'SET_DUAL_CARRIAGE', self.cmd_SET_DUAL_CARRIAGE,
desc=self.cmd_SET_DUAL_CARRIAGE_help)
def get_steppers(self):
rails = self.rails
if self.dual_carriage_axis is not None:
dca = self.dual_carriage_axis
rails = rails[:dca] + self.dual_carriage_rails + rails[dca+1:]
return [s for rail in rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
return [stepper_positions[rail.get_name()] for rail in self.rails]
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
if i in homing_axes:
self.limits[i] = rail.get_range()
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limits[2] = (1.0, -1.0)
def _home_axis(self, homing_state, axis, rail):
# Determine movement
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
else:
forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def home(self, homing_state):
# Each axis is homed independently and in order
for axis in homing_state.get_axes():
if axis == self.dual_carriage_axis:
dc1, dc2 = self.dual_carriage_rails
altc = self.rails[axis] == dc2
self._activate_carriage(0)
self._home_axis(homing_state, axis, dc1)
self._activate_carriage(1)
self._home_axis(homing_state, axis, dc2)
self._activate_carriage(altc)
else:
self._home_axis(homing_state, axis, self.rails[axis])
def _motor_off(self, print_time):
self.limits = [(1.0, -1.0)] * 3
def _check_endstops(self, move):
end_pos = move.end_pos
for i in (0, 1, 2):
if (move.axes_d[i]
and (end_pos[i] < self.limits[i][0]
or end_pos[i] > self.limits[i][1])):
if self.limits[i][0] > self.limits[i][1]:
raise move.move_error("Must home axis first")
raise move.move_error()
def check_move(self, move):
limits = self.limits
xpos, ypos = move.end_pos[:2]
if (xpos < limits[0][0] or xpos > limits[0][1]
or ypos < limits[1][0] or ypos > limits[1][1]):
self._check_endstops(move)
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
self._check_endstops(move)
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
def get_status(self, eventtime):
axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
return {
'homed_axes': "".join(axes),
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
# Dual carriage support
def _activate_carriage(self, carriage):
toolhead = self.printer.lookup_object('toolhead')
toolhead.flush_step_generation()
dc_rail = self.dual_carriage_rails[carriage]
dc_axis = self.dual_carriage_axis
self.rails[dc_axis].set_trapq(None)
dc_rail.set_trapq(toolhead.get_trapq())
self.rails[dc_axis] = dc_rail
pos = toolhead.get_position()
pos[dc_axis] = dc_rail.get_commanded_position()
toolhead.set_position(pos)
if self.limits[dc_axis][0] <= self.limits[dc_axis][1]:
self.limits[dc_axis] = dc_rail.get_range()
cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"
def cmd_SET_DUAL_CARRIAGE(self, gcmd):
carriage = gcmd.get_int('CARRIAGE', minval=0, maxval=1)
self._activate_carriage(carriage)
def load_kinematics(toolhead, config):
return CartKinematics(toolhead, config)

99
klippy/kinematics/corexy.py Normal file
View File

@@ -0,0 +1,99 @@
# Code for handling the kinematics of corexy robots
#
# Copyright (C) 2017-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math
import stepper
class CoreXYKinematics:
def __init__(self, toolhead, config):
# Setup axis rails
self.rails = [stepper.LookupMultiRail(config.getsection('stepper_' + n))
for n in 'xyz']
for s in self.rails[1].get_steppers():
self.rails[0].get_endstops()[0][0].add_stepper(s)
for s in self.rails[0].get_steppers():
self.rails[1].get_endstops()[0][0].add_stepper(s)
self.rails[0].setup_itersolve('corexy_stepper_alloc', b'+')
self.rails[1].setup_itersolve('corexy_stepper_alloc', b'-')
self.rails[2].setup_itersolve('cartesian_stepper_alloc', b'z')
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
config.get_printer().register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat(
'max_z_accel', max_accel, above=0., maxval=max_accel)
self.limits = [(1.0, -1.0)] * 3
ranges = [r.get_range() for r in self.rails]
self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
pos = [stepper_positions[rail.get_name()] for rail in self.rails]
return [0.5 * (pos[0] + pos[1]), 0.5 * (pos[0] - pos[1]), pos[2]]
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
if i in homing_axes:
self.limits[i] = rail.get_range()
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limits[2] = (1.0, -1.0)
def home(self, homing_state):
# Each axis is homed independently and in order
for axis in homing_state.get_axes():
rail = self.rails[axis]
# Determine movement
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
else:
forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def _motor_off(self, print_time):
self.limits = [(1.0, -1.0)] * 3
def _check_endstops(self, move):
end_pos = move.end_pos
for i in (0, 1, 2):
if (move.axes_d[i]
and (end_pos[i] < self.limits[i][0]
or end_pos[i] > self.limits[i][1])):
if self.limits[i][0] > self.limits[i][1]:
raise move.move_error("Must home axis first")
raise move.move_error()
def check_move(self, move):
limits = self.limits
xpos, ypos = move.end_pos[:2]
if (xpos < limits[0][0] or xpos > limits[0][1]
or ypos < limits[1][0] or ypos > limits[1][1]):
self._check_endstops(move)
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
self._check_endstops(move)
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
def get_status(self, eventtime):
axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
return {
'homed_axes': "".join(axes),
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def load_kinematics(toolhead, config):
return CoreXYKinematics(toolhead, config)

BIN
klippy/kinematics/corexy.pyc Normal file
View File

Binary file not shown.

100
klippy/kinematics/corexz.py Normal file
View File

@@ -0,0 +1,100 @@
# Code for handling the kinematics of corexz robots
#
# Copyright (C) 2020 Maks Zolin <mzolin@vorondesign.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math
import stepper
class CoreXZKinematics:
def __init__(self, toolhead, config):
# Setup axis rails
self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
stepper.PrinterRail(config.getsection('stepper_y')),
stepper.PrinterRail(config.getsection('stepper_z')) ]
self.rails[0].get_endstops()[0][0].add_stepper(
self.rails[2].get_steppers()[0])
self.rails[2].get_endstops()[0][0].add_stepper(
self.rails[0].get_steppers()[0])
self.rails[0].setup_itersolve('corexz_stepper_alloc', b'+')
self.rails[1].setup_itersolve('cartesian_stepper_alloc', b'y')
self.rails[2].setup_itersolve('corexz_stepper_alloc', b'-')
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
config.get_printer().register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat(
'max_z_accel', max_accel, above=0., maxval=max_accel)
self.limits = [(1.0, -1.0)] * 3
ranges = [r.get_range() for r in self.rails]
self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
pos = [stepper_positions[rail.get_name()] for rail in self.rails]
return [0.5 * (pos[0] + pos[2]), pos[1], 0.5 * (pos[0] - pos[2])]
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
if i in homing_axes:
self.limits[i] = rail.get_range()
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limits[2] = (1.0, -1.0)
def home(self, homing_state):
# Each axis is homed independently and in order
for axis in homing_state.get_axes():
rail = self.rails[axis]
# Determine movement
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
else:
forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def _motor_off(self, print_time):
self.limits = [(1.0, -1.0)] * 3
def _check_endstops(self, move):
end_pos = move.end_pos
for i in (0, 1, 2):
if (move.axes_d[i]
and (end_pos[i] < self.limits[i][0]
or end_pos[i] > self.limits[i][1])):
if self.limits[i][0] > self.limits[i][1]:
raise move.move_error("Must home axis first")
raise move.move_error()
def check_move(self, move):
limits = self.limits
xpos, ypos = move.end_pos[:2]
if (xpos < limits[0][0] or xpos > limits[0][1]
or ypos < limits[1][0] or ypos > limits[1][1]):
self._check_endstops(move)
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
self._check_endstops(move)
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
def get_status(self, eventtime):
axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
return {
'homed_axes': "".join(axes),
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def load_kinematics(toolhead, config):
return CoreXZKinematics(toolhead, config)

235
klippy/kinematics/delta.py Normal file
View File

@@ -0,0 +1,235 @@
# Code for handling the kinematics of linear delta robots
#
# Copyright (C) 2016-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging
import stepper, mathutil
# Slow moves once the ratio of tower to XY movement exceeds SLOW_RATIO
SLOW_RATIO = 3.
class DeltaKinematics:
def __init__(self, toolhead, config):
# Setup tower rails
stepper_configs = [config.getsection('stepper_' + a) for a in 'abc']
rail_a = stepper.LookupMultiRail(
stepper_configs[0], need_position_minmax = False)
a_endstop = rail_a.get_homing_info().position_endstop
rail_b = stepper.LookupMultiRail(
stepper_configs[1], need_position_minmax = False,
default_position_endstop=a_endstop)
rail_c = stepper.LookupMultiRail(
stepper_configs[2], need_position_minmax = False,
default_position_endstop=a_endstop)
self.rails = [rail_a, rail_b, rail_c]
config.get_printer().register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup max velocity
self.max_velocity, self.max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', self.max_velocity,
above=0., maxval=self.max_velocity)
self.max_z_accel = config.getfloat('max_z_accel', self.max_accel,
above=0., maxval=self.max_accel)
# Read radius and arm lengths
self.radius = radius = config.getfloat('delta_radius', above=0.)
print_radius = config.getfloat('print_radius', radius, above=0.)
arm_length_a = stepper_configs[0].getfloat('arm_length', above=radius)
self.arm_lengths = 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.abs_endstops = [(rail.get_homing_info().position_endstop
+ math.sqrt(arm2 - radius**2))
for rail, arm2 in zip(self.rails, self.arm2)]
# Determine tower locations in cartesian space
self.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 self.angles]
for r, a, t in zip(self.rails, self.arm2, self.towers):
r.setup_itersolve('delta_stepper_alloc', a, t[0], t[1])
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
# Setup boundary checks
self.need_home = True
self.limit_xy2 = -1.
self.home_position = tuple(
self._actuator_to_cartesian(self.abs_endstops))
self.max_z = min([rail.get_homing_info().position_endstop
for rail in self.rails])
self.min_z = config.getfloat('minimum_z_position', 0, maxval=self.max_z)
self.limit_z = min([ep - arm
for ep, arm in zip(self.abs_endstops, arm_lengths)])
logging.info(
"Delta max build height %.2fmm (radius tapered above %.2fmm)"
% (self.max_z, self.limit_z))
# Find the point where an XY move could result in excessive
# tower movement
half_min_step_dist = min([r.get_steppers()[0].get_step_dist()
for r in self.rails]) * .5
min_arm_length = min(arm_lengths)
def ratio_to_xy(ratio):
return (ratio * math.sqrt(min_arm_length**2 / (ratio**2 + 1.)
- half_min_step_dist**2)
+ half_min_step_dist - radius)
self.slow_xy2 = ratio_to_xy(SLOW_RATIO)**2
self.very_slow_xy2 = ratio_to_xy(2. * SLOW_RATIO)**2
self.max_xy2 = min(print_radius, min_arm_length - radius,
ratio_to_xy(4. * SLOW_RATIO))**2
max_xy = math.sqrt(self.max_xy2)
logging.info("Delta max build radius %.2fmm (moves slowed past %.2fmm"
" and %.2fmm)"
% (max_xy, math.sqrt(self.slow_xy2),
math.sqrt(self.very_slow_xy2)))
self.axes_min = toolhead.Coord(-max_xy, -max_xy, self.min_z, 0.)
self.axes_max = toolhead.Coord(max_xy, max_xy, self.max_z, 0.)
self.set_position([0., 0., 0.], ())
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def _actuator_to_cartesian(self, spos):
sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
return mathutil.trilateration(sphere_coords, self.arm2)
def calc_position(self, stepper_positions):
spos = [stepper_positions[rail.get_name()] for rail in self.rails]
return self._actuator_to_cartesian(spos)
def set_position(self, newpos, homing_axes):
for rail in self.rails:
rail.set_position(newpos)
self.limit_xy2 = -1.
if tuple(homing_axes) == (0, 1, 2):
self.need_home = False
def home(self, homing_state):
# All axes are homed simultaneously
homing_state.set_axes([0, 1, 2])
forcepos = list(self.home_position)
forcepos[2] = -1.5 * math.sqrt(max(self.arm2)-self.max_xy2)
homing_state.home_rails(self.rails, forcepos, self.home_position)
def _motor_off(self, print_time):
self.limit_xy2 = -1.
self.need_home = True
def check_move(self, move):
end_pos = move.end_pos
end_xy2 = end_pos[0]**2 + end_pos[1]**2
if end_xy2 <= self.limit_xy2 and not move.axes_d[2]:
# Normal XY move
return
if self.need_home:
raise move.move_error("Must home first")
end_z = end_pos[2]
limit_xy2 = self.max_xy2
if end_z > self.limit_z:
limit_xy2 = min(limit_xy2, (self.max_z - end_z)**2)
if end_xy2 > limit_xy2 or end_z > self.max_z or end_z < self.min_z:
# Move out of range - verify not a homing move
if (end_pos[:2] != self.home_position[:2]
or end_z < self.min_z or end_z > self.home_position[2]):
raise move.move_error()
limit_xy2 = -1.
if move.axes_d[2]:
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(self.max_z_velocity * z_ratio,
self.max_z_accel * z_ratio)
limit_xy2 = -1.
# Limit the speed/accel of this move if is is at the extreme
# end of the build envelope
extreme_xy2 = max(end_xy2, move.start_pos[0]**2 + move.start_pos[1]**2)
if extreme_xy2 > self.slow_xy2:
r = 0.5
if extreme_xy2 > self.very_slow_xy2:
r = 0.25
move.limit_speed(self.max_velocity * r, self.max_accel * r)
limit_xy2 = -1.
self.limit_xy2 = min(limit_xy2, self.slow_xy2)
def get_status(self, eventtime):
return {
'homed_axes': '' if self.need_home else 'xyz',
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def get_calibration(self):
endstops = [rail.get_homing_info().position_endstop
for rail in self.rails]
stepdists = [rail.get_steppers()[0].get_step_dist()
for rail in self.rails]
return DeltaCalibration(self.radius, self.angles, self.arm_lengths,
endstops, stepdists)
# Delta parameter calibration for DELTA_CALIBRATE tool
class DeltaCalibration:
def __init__(self, radius, angles, arms, endstops, stepdists):
self.radius = radius
self.angles = angles
self.arms = arms
self.endstops = endstops
self.stepdists = stepdists
# Calculate the XY cartesian coordinates of the delta towers
radian_angles = [math.radians(a) for a in angles]
self.towers = [(math.cos(a) * radius, math.sin(a) * radius)
for a in radian_angles]
# Calculate the absolute Z height of each tower endstop
radius2 = radius**2
self.abs_endstops = [e + math.sqrt(a**2 - radius2)
for e, a in zip(endstops, arms)]
def coordinate_descent_params(self, is_extended):
# Determine adjustment parameters (for use with coordinate_descent)
adj_params = ('radius', 'angle_a', 'angle_b',
'endstop_a', 'endstop_b', 'endstop_c')
if is_extended:
adj_params += ('arm_a', 'arm_b', 'arm_c')
params = { 'radius': self.radius }
for i, axis in enumerate('abc'):
params['angle_'+axis] = self.angles[i]
params['arm_'+axis] = self.arms[i]
params['endstop_'+axis] = self.endstops[i]
params['stepdist_'+axis] = self.stepdists[i]
return adj_params, params
def new_calibration(self, params):
# Create a new calibration object from coordinate_descent params
radius = params['radius']
angles = [params['angle_'+a] for a in 'abc']
arms = [params['arm_'+a] for a in 'abc']
endstops = [params['endstop_'+a] for a in 'abc']
stepdists = [params['stepdist_'+a] for a in 'abc']
return DeltaCalibration(radius, angles, arms, endstops, stepdists)
def get_position_from_stable(self, stable_position):
# Return cartesian coordinates for the given stable_position
sphere_coords = [
(t[0], t[1], es - sp * sd)
for sd, t, es, sp in zip(self.stepdists, self.towers,
self.abs_endstops, stable_position) ]
return mathutil.trilateration(sphere_coords, [a**2 for a in self.arms])
def calc_stable_position(self, coord):
# Return a stable_position from a cartesian coordinate
steppos = [
math.sqrt(a**2 - (t[0]-coord[0])**2 - (t[1]-coord[1])**2) + coord[2]
for t, a in zip(self.towers, self.arms) ]
return [(ep - sp) / sd
for sd, ep, sp in zip(self.stepdists,
self.abs_endstops, steppos)]
def save_state(self, configfile):
# Save the current parameters (for use with SAVE_CONFIG)
configfile.set('printer', 'delta_radius', "%.6f" % (self.radius,))
for i, axis in enumerate('abc'):
configfile.set('stepper_'+axis, 'angle', "%.6f" % (self.angles[i],))
configfile.set('stepper_'+axis, 'arm_length',
"%.6f" % (self.arms[i],))
configfile.set('stepper_'+axis, 'position_endstop',
"%.6f" % (self.endstops[i],))
gcode = configfile.get_printer().lookup_object("gcode")
gcode.respond_info(
"stepper_a: position_endstop: %.6f angle: %.6f arm_length: %.6f\n"
"stepper_b: position_endstop: %.6f angle: %.6f arm_length: %.6f\n"
"stepper_c: position_endstop: %.6f angle: %.6f arm_length: %.6f\n"
"delta_radius: %.6f"
% (self.endstops[0], self.angles[0], self.arms[0],
self.endstops[1], self.angles[1], self.arms[1],
self.endstops[2], self.angles[2], self.arms[2],
self.radius))
def load_kinematics(toolhead, config):
return DeltaKinematics(toolhead, config)

334
klippy/kinematics/extruder.py Normal file
View File

@@ -0,0 +1,334 @@
# Code for handling printer nozzle extruders
#
# Copyright (C) 2016-2022 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging
import stepper, chelper
class ExtruderStepper:
def __init__(self, config):
self.printer = config.get_printer()
self.name = config.get_name().split()[-1]
self.pressure_advance = self.pressure_advance_smooth_time = 0.
# Setup stepper
self.stepper = stepper.PrinterStepper(config)
ffi_main, ffi_lib = chelper.get_ffi()
self.sk_extruder = ffi_main.gc(ffi_lib.extruder_stepper_alloc(),
ffi_lib.free)
self.stepper.set_stepper_kinematics(self.sk_extruder)
# Register commands
self.printer.register_event_handler("klippy:connect",
self._handle_connect)
gcode = self.printer.lookup_object('gcode')
if self.name == 'extruder':
gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER", None,
self.cmd_default_SET_PRESSURE_ADVANCE,
desc=self.cmd_SET_PRESSURE_ADVANCE_help)
gcode.register_mux_command("SET_PRESSURE_ADVANCE", "EXTRUDER",
self.name, self.cmd_SET_PRESSURE_ADVANCE,
desc=self.cmd_SET_PRESSURE_ADVANCE_help)
gcode.register_mux_command("SET_EXTRUDER_ROTATION_DISTANCE", "EXTRUDER",
self.name, self.cmd_SET_E_ROTATION_DISTANCE,
desc=self.cmd_SET_E_ROTATION_DISTANCE_help)
gcode.register_mux_command("SYNC_EXTRUDER_MOTION", "EXTRUDER",
self.name, self.cmd_SYNC_EXTRUDER_MOTION,
desc=self.cmd_SYNC_EXTRUDER_MOTION_help)
gcode.register_mux_command("SET_EXTRUDER_STEP_DISTANCE", "EXTRUDER",
self.name, self.cmd_SET_E_STEP_DISTANCE,
desc=self.cmd_SET_E_STEP_DISTANCE_help)
gcode.register_mux_command("SYNC_STEPPER_TO_EXTRUDER", "STEPPER",
self.name, self.cmd_SYNC_STEPPER_TO_EXTRUDER,
desc=self.cmd_SYNC_STEPPER_TO_EXTRUDER_help)
def _handle_connect(self):
toolhead = self.printer.lookup_object('toolhead')
toolhead.register_step_generator(self.stepper.generate_steps)
def get_status(self, eventtime):
return {'pressure_advance': self.pressure_advance,
'smooth_time': self.pressure_advance_smooth_time}
def find_past_position(self, print_time):
mcu_pos = self.stepper.get_past_mcu_position(print_time)
return self.stepper.mcu_to_commanded_position(mcu_pos)
def sync_to_extruder(self, extruder_name):
toolhead = self.printer.lookup_object('toolhead')
toolhead.flush_step_generation()
if not extruder_name:
self.stepper.set_trapq(None)
return
extruder = self.printer.lookup_object(extruder_name, None)
if extruder is None or not isinstance(extruder, PrinterExtruder):
raise self.printer.command_error("'%s' is not a valid extruder."
% (extruder_name,))
self.stepper.set_position([extruder.last_position, 0., 0.])
self.stepper.set_trapq(extruder.get_trapq())
def _set_pressure_advance(self, pressure_advance, smooth_time):
old_smooth_time = self.pressure_advance_smooth_time
if not self.pressure_advance:
old_smooth_time = 0.
new_smooth_time = smooth_time
if not pressure_advance:
new_smooth_time = 0.
toolhead = self.printer.lookup_object("toolhead")
toolhead.note_step_generation_scan_time(new_smooth_time * .5,
old_delay=old_smooth_time * .5)
ffi_main, ffi_lib = chelper.get_ffi()
espa = ffi_lib.extruder_set_pressure_advance
espa(self.sk_extruder, pressure_advance, new_smooth_time)
self.pressure_advance = pressure_advance
self.pressure_advance_smooth_time = smooth_time
cmd_SET_PRESSURE_ADVANCE_help = "Set pressure advance parameters"
def cmd_default_SET_PRESSURE_ADVANCE(self, gcmd):
extruder = self.printer.lookup_object('toolhead').get_extruder()
if extruder.extruder_stepper is None:
raise gcmd.error("Active extruder does not have a stepper")
strapq = extruder.extruder_stepper.stepper.get_trapq()
if strapq is not extruder.get_trapq():
raise gcmd.error("Unable to infer active extruder stepper")
extruder.extruder_stepper.cmd_SET_PRESSURE_ADVANCE(gcmd)
def cmd_SET_PRESSURE_ADVANCE(self, gcmd):
pressure_advance = gcmd.get_float('ADVANCE', self.pressure_advance,
minval=0.)
smooth_time = gcmd.get_float('SMOOTH_TIME',
self.pressure_advance_smooth_time,
minval=0., maxval=.200)
self._set_pressure_advance(pressure_advance, smooth_time)
msg = ("pressure_advance: %.6f\n"
"pressure_advance_smooth_time: %.6f"
% (pressure_advance, smooth_time))
self.printer.set_rollover_info(self.name, "%s: %s" % (self.name, msg))
gcmd.respond_info(msg, log=False)
cmd_SET_E_ROTATION_DISTANCE_help = "Set extruder rotation distance"
def cmd_SET_E_ROTATION_DISTANCE(self, gcmd):
rotation_dist = gcmd.get_float('DISTANCE', None)
if rotation_dist is not None:
if not rotation_dist:
raise gcmd.error("Rotation distance can not be zero")
invert_dir, orig_invert_dir = self.stepper.get_dir_inverted()
next_invert_dir = orig_invert_dir
if rotation_dist < 0.:
next_invert_dir = not orig_invert_dir
rotation_dist = -rotation_dist
toolhead = self.printer.lookup_object('toolhead')
toolhead.flush_step_generation()
self.stepper.set_rotation_distance(rotation_dist)
self.stepper.set_dir_inverted(next_invert_dir)
else:
rotation_dist, spr = self.stepper.get_rotation_distance()
invert_dir, orig_invert_dir = self.stepper.get_dir_inverted()
if invert_dir != orig_invert_dir:
rotation_dist = -rotation_dist
gcmd.respond_info("Extruder '%s' rotation distance set to %0.6f"
% (self.name, rotation_dist))
cmd_SYNC_EXTRUDER_MOTION_help = "Set extruder stepper motion queue"
def cmd_SYNC_EXTRUDER_MOTION(self, gcmd):
ename = gcmd.get('MOTION_QUEUE')
self.sync_to_extruder(ename)
gcmd.respond_info("Extruder stepper now syncing with '%s'" % (ename,))
cmd_SET_E_STEP_DISTANCE_help = "Set extruder step distance"
def cmd_SET_E_STEP_DISTANCE(self, gcmd):
step_dist = gcmd.get_float('DISTANCE', None, above=0.)
if step_dist is not None:
toolhead = self.printer.lookup_object('toolhead')
toolhead.flush_step_generation()
rd, steps_per_rotation = self.stepper.get_rotation_distance()
self.stepper.set_rotation_distance(step_dist * steps_per_rotation)
else:
step_dist = self.stepper.get_step_dist()
gcmd.respond_info("Extruder '%s' step distance set to %0.6f"
% (self.name, step_dist))
cmd_SYNC_STEPPER_TO_EXTRUDER_help = "Set extruder stepper"
def cmd_SYNC_STEPPER_TO_EXTRUDER(self, gcmd):
ename = gcmd.get('EXTRUDER')
self.sync_to_extruder(ename)
gcmd.respond_info("Extruder stepper now syncing with '%s'" % (ename,))
# Tracking for hotend heater, extrusion motion queue, and extruder stepper
class PrinterExtruder:
def __init__(self, config, extruder_num):
self.printer = config.get_printer()
self.name = config.get_name()
self.last_position = 0.
# Setup hotend heater
shared_heater = config.get('shared_heater', None)
pheaters = self.printer.load_object(config, 'heaters')
gcode_id = 'T%d' % (extruder_num,)
if shared_heater is None:
self.heater = pheaters.setup_heater(config, gcode_id)
else:
config.deprecate('shared_heater')
self.heater = pheaters.lookup_heater(shared_heater)
# Setup kinematic checks
self.nozzle_diameter = config.getfloat('nozzle_diameter', above=0.)
filament_diameter = config.getfloat(
'filament_diameter', minval=self.nozzle_diameter)
self.filament_area = math.pi * (filament_diameter * .5)**2
def_max_cross_section = 4. * self.nozzle_diameter**2
def_max_extrude_ratio = def_max_cross_section / self.filament_area
max_cross_section = config.getfloat(
'max_extrude_cross_section', def_max_cross_section, above=0.)
self.max_extrude_ratio = max_cross_section / self.filament_area
logging.info("Extruder max_extrude_ratio=%.6f", self.max_extrude_ratio)
toolhead = self.printer.lookup_object('toolhead')
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_e_velocity = config.getfloat(
'max_extrude_only_velocity', max_velocity * def_max_extrude_ratio
, above=0.)
self.max_e_accel = config.getfloat(
'max_extrude_only_accel', max_accel * def_max_extrude_ratio
, above=0.)
self.max_e_dist = config.getfloat(
'max_extrude_only_distance', 50., minval=0.)
self.instant_corner_v = config.getfloat(
'instantaneous_corner_velocity', 1., minval=0.)
# Setup extruder trapq (trapezoidal motion queue)
ffi_main, ffi_lib = chelper.get_ffi()
self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free)
self.trapq_append = ffi_lib.trapq_append
self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves
# Setup extruder stepper
self.extruder_stepper = None
if (config.get('step_pin', None) is not None
or config.get('dir_pin', None) is not None
or config.get('rotation_distance', None) is not None):
self.extruder_stepper = ExtruderStepper(config)
self.extruder_stepper.stepper.set_trapq(self.trapq)
pa = config.getfloat('pressure_advance', 0., minval=0.)
smooth_time = config.getfloat('pressure_advance_smooth_time',
0.040, above=0., maxval=.200)
self.extruder_stepper._set_pressure_advance(pa, smooth_time)
# Register commands
gcode = self.printer.lookup_object('gcode')
if self.name == 'extruder':
toolhead.set_extruder(self, 0.)
gcode.register_command("M104", self.cmd_M104)
gcode.register_command("M109", self.cmd_M109)
gcode.register_mux_command("ACTIVATE_EXTRUDER", "EXTRUDER",
self.name, self.cmd_ACTIVATE_EXTRUDER,
desc=self.cmd_ACTIVATE_EXTRUDER_help)
def update_move_time(self, flush_time):
self.trapq_finalize_moves(self.trapq, flush_time)
def get_status(self, eventtime):
sts = self.heater.get_status(eventtime)
sts['can_extrude'] = self.heater.can_extrude
if self.extruder_stepper is not None:
sts.update(self.extruder_stepper.get_status(eventtime))
return sts
def get_name(self):
return self.name
def get_heater(self):
return self.heater
def get_trapq(self):
return self.trapq
def stats(self, eventtime):
return self.heater.stats(eventtime)
def check_move(self, move):
axis_r = move.axes_r[3]
if not self.heater.can_extrude:
raise self.printer.command_error(
"Extrude below minimum temp\n"
"See the 'min_extrude_temp' config option for details")
if (not move.axes_d[0] and not move.axes_d[1]) or axis_r < 0.:
# Extrude only move (or retraction move) - limit accel and velocity
if abs(move.axes_d[3]) > self.max_e_dist:
raise self.printer.command_error(
"Extrude only move too long (%.3fmm vs %.3fmm)\n"
"See the 'max_extrude_only_distance' config"
" option for details" % (move.axes_d[3], self.max_e_dist))
inv_extrude_r = 1. / abs(axis_r)
move.limit_speed(self.max_e_velocity * inv_extrude_r,
self.max_e_accel * inv_extrude_r)
elif axis_r > self.max_extrude_ratio:
if move.axes_d[3] <= self.nozzle_diameter * self.max_extrude_ratio:
# Permit extrusion if amount extruded is tiny
return
area = axis_r * self.filament_area
logging.debug("Overextrude: %s vs %s (area=%.3f dist=%.3f)",
axis_r, self.max_extrude_ratio, area, move.move_d)
raise self.printer.command_error(
"Move exceeds maximum extrusion (%.3fmm^2 vs %.3fmm^2)\n"
"See the 'max_extrude_cross_section' config option for details"
% (area, self.max_extrude_ratio * self.filament_area))
def calc_junction(self, prev_move, move):
diff_r = move.axes_r[3] - prev_move.axes_r[3]
if diff_r:
return (self.instant_corner_v / abs(diff_r))**2
return move.max_cruise_v2
def move(self, print_time, move):
axis_r = move.axes_r[3]
accel = move.accel * axis_r
start_v = move.start_v * axis_r
cruise_v = move.cruise_v * axis_r
can_pressure_advance = False
if axis_r > 0. and (move.axes_d[0] or move.axes_d[1]):
can_pressure_advance = True
# Queue movement (x is extruder movement, y is pressure advance flag)
self.trapq_append(self.trapq, print_time,
move.accel_t, move.cruise_t, move.decel_t,
move.start_pos[3], 0., 0.,
1., can_pressure_advance, 0.,
start_v, cruise_v, accel)
self.last_position = move.end_pos[3]
def find_past_position(self, print_time):
if self.extruder_stepper is None:
return 0.
return self.extruder_stepper.find_past_position(print_time)
def cmd_M104(self, gcmd, wait=False):
# Set Extruder Temperature
temp = gcmd.get_float('S', 0.)
index = gcmd.get_int('T', None, minval=0)
if index is not None:
section = 'extruder'
if index:
section = 'extruder%d' % (index,)
extruder = self.printer.lookup_object(section, None)
if extruder is None:
if temp <= 0.:
return
raise gcmd.error("Extruder not configured")
else:
extruder = self.printer.lookup_object('toolhead').get_extruder()
pheaters = self.printer.lookup_object('heaters')
pheaters.set_temperature(extruder.get_heater(), temp, wait)
def cmd_M109(self, gcmd):
# Set Extruder Temperature and Wait
self.cmd_M104(gcmd, wait=True)
cmd_ACTIVATE_EXTRUDER_help = "Change the active extruder"
def cmd_ACTIVATE_EXTRUDER(self, gcmd):
toolhead = self.printer.lookup_object('toolhead')
if toolhead.get_extruder() is self:
gcmd.respond_info("Extruder %s already active" % (self.name,))
return
gcmd.respond_info("Activating extruder %s" % (self.name,))
toolhead.flush_step_generation()
toolhead.set_extruder(self, self.last_position)
self.printer.send_event("extruder:activate_extruder")
# Dummy extruder class used when a printer has no extruder at all
class DummyExtruder:
def __init__(self, printer):
self.printer = printer
def update_move_time(self, flush_time):
pass
def check_move(self, move):
raise move.move_error("Extrude when no extruder present")
def find_past_position(self, print_time):
return 0.
def calc_junction(self, prev_move, move):
return move.max_cruise_v2
def get_name(self):
return ""
def get_heater(self):
raise self.printer.command_error("Extruder not configured")
def get_trapq(self):
raise self.printer.command_error("Extruder not configured")
def add_printer_objects(config):
printer = config.get_printer()
for i in range(99):
section = 'extruder'
if i:
section = 'extruder%d' % (i,)
if not config.has_section(section):
break
pe = PrinterExtruder(config.getsection(section), i)
printer.add_object(section, pe)

BIN
klippy/kinematics/extruder.pyc Normal file
View File

Binary file not shown.

136
klippy/kinematics/hybrid_corexy.py Normal file
View File

@@ -0,0 +1,136 @@
# Code for handling the kinematics of hybrid-corexy robots
#
# Copyright (C) 2021 Fabrice Gallet <tircown@gmail.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging
import stepper
from . import idex_modes
# The hybrid-corexy kinematic is also known as Markforged kinematics
class HybridCoreXYKinematics:
def __init__(self, toolhead, config):
self.printer = config.get_printer()
printer_config = config.getsection('printer')
# itersolve parameters
self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
stepper.LookupMultiRail(config.getsection('stepper_y')),
stepper.LookupMultiRail(config.getsection('stepper_z'))]
self.rails[1].get_endstops()[0][0].add_stepper(
self.rails[0].get_steppers()[0])
self.rails[0].setup_itersolve('corexy_stepper_alloc', b'-')
self.rails[1].setup_itersolve('cartesian_stepper_alloc', b'y')
self.rails[2].setup_itersolve('cartesian_stepper_alloc', b'z')
ranges = [r.get_range() for r in self.rails]
self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
self.dc_module = None
if config.has_section('dual_carriage'):
dc_config = config.getsection('dual_carriage')
# dummy for cartesian config users
dc_config.getchoice('axis', {'x': 'x'}, default='x')
# setup second dual carriage rail
self.rails.append(stepper.PrinterRail(dc_config))
self.rails[1].get_endstops()[0][0].add_stepper(
self.rails[3].get_steppers()[0])
self.rails[3].setup_itersolve('cartesian_stepper_alloc', b'y')
dc_rail_0 = idex_modes.DualCarriagesRail(
self.printer, self.rails[0], axis=0, active=True,
stepper_alloc_active=('corexy_stepper_alloc', b'-'),
stepper_alloc_inactive=('cartesian_reverse_stepper_alloc',b'y'))
dc_rail_1 = idex_modes.DualCarriagesRail(
self.printer, self.rails[3], axis=0, active=False,
stepper_alloc_active=('corexy_stepper_alloc', b'+'),
stepper_alloc_inactive=('cartesian_stepper_alloc', b'y'))
self.dc_module = idex_modes.DualCarriages(self.printer,
dc_rail_0, dc_rail_1, axis=0)
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
self.printer.register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat(
'max_z_accel', max_accel, above=0., maxval=max_accel)
self.limits = [(1.0, -1.0)] * 3
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
pos = [stepper_positions[rail.get_name()] for rail in self.rails]
if (self.dc_module is not None and 'CARRIAGE_1' == \
self.dc_module.get_status()['active_carriage']):
return [pos[0] - pos[1], pos[1], pos[2]]
else:
return [pos[0] + pos[1], pos[1], pos[2]]
def update_limits(self, i, range):
self.limits[i] = range
def override_rail(self, i, rail):
self.rails[i] = rail
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
if i in homing_axes:
self.limits[i] = rail.get_range()
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limits[2] = (1.0, -1.0)
def _home_axis(self, homing_state, axis, rail):
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
else:
forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def home(self, homing_state):
for axis in homing_state.get_axes():
if (self.dc_module is not None and axis == 0):
self.dc_module.save_idex_state()
for i in [0,1]:
self.dc_module.toggle_active_dc_rail(i)
self._home_axis(homing_state, axis, self.rails[0])
self.dc_module.restore_idex_state()
else:
self._home_axis(homing_state, axis, self.rails[axis])
def _motor_off(self, print_time):
self.limits = [(1.0, -1.0)] * 3
def _check_endstops(self, move):
end_pos = move.end_pos
for i in (0, 1, 2):
if (move.axes_d[i]
and (end_pos[i] < self.limits[i][0]
or end_pos[i] > self.limits[i][1])):
if self.limits[i][0] > self.limits[i][1]:
raise move.move_error("Must home axis first")
raise move.move_error()
def check_move(self, move):
limits = self.limits
xpos, ypos = move.end_pos[:2]
if (xpos < limits[0][0] or xpos > limits[0][1]
or ypos < limits[1][0] or ypos > limits[1][1]):
self._check_endstops(move)
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
self._check_endstops(move)
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
def get_status(self, eventtime):
axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
return {
'homed_axes': "".join(axes),
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max
}
def load_kinematics(toolhead, config):
return HybridCoreXYKinematics(toolhead, config)

136
klippy/kinematics/hybrid_corexz.py Normal file
View File

@@ -0,0 +1,136 @@
# Code for handling the kinematics of hybrid-corexz robots
#
# Copyright (C) 2021 Fabrice Gallet <tircown@gmail.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging
import stepper
from . import idex_modes
# The hybrid-corexz kinematic is also known as Markforged kinematics
class HybridCoreXZKinematics:
def __init__(self, toolhead, config):
self.printer = config.get_printer()
printer_config = config.getsection('printer')
# itersolve parameters
self.rails = [ stepper.PrinterRail(config.getsection('stepper_x')),
stepper.LookupMultiRail(config.getsection('stepper_y')),
stepper.LookupMultiRail(config.getsection('stepper_z'))]
self.rails[2].get_endstops()[0][0].add_stepper(
self.rails[0].get_steppers()[0])
self.rails[0].setup_itersolve('corexz_stepper_alloc', b'-')
self.rails[1].setup_itersolve('cartesian_stepper_alloc', b'y')
self.rails[2].setup_itersolve('cartesian_stepper_alloc', b'z')
ranges = [r.get_range() for r in self.rails]
self.axes_min = toolhead.Coord(*[r[0] for r in ranges], e=0.)
self.axes_max = toolhead.Coord(*[r[1] for r in ranges], e=0.)
self.dc_module = None
if config.has_section('dual_carriage'):
dc_config = config.getsection('dual_carriage')
# dummy for cartesian config users
dc_config.getchoice('axis', {'x': 'x'}, default='x')
# setup second dual carriage rail
self.rails.append(stepper.PrinterRail(dc_config))
self.rails[2].get_endstops()[0][0].add_stepper(
self.rails[3].get_steppers()[0])
self.rails[3].setup_itersolve('cartesian_stepper_alloc', b'z')
dc_rail_0 = idex_modes.DualCarriagesRail(
self.printer, self.rails[0], axis=0, active=True,
stepper_alloc_active=('corexz_stepper_alloc', b'-'),
stepper_alloc_inactive=('cartesian_reverse_stepper_alloc',b'z'))
dc_rail_1 = idex_modes.DualCarriagesRail(
self.printer, self.rails[3], axis=0, active=False,
stepper_alloc_active=('corexz_stepper_alloc', b'+'),
stepper_alloc_inactive=('cartesian_stepper_alloc', b'z'))
self.dc_module = idex_modes.DualCarriages(self.printer,
dc_rail_0, dc_rail_1, axis=0)
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
self.printer.register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat(
'max_z_accel', max_accel, above=0., maxval=max_accel)
self.limits = [(1.0, -1.0)] * 3
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
pos = [stepper_positions[rail.get_name()] for rail in self.rails]
if (self.dc_module is not None and 'CARRIAGE_1' == \
self.dc_module.get_status()['active_carriage']):
return [pos[0] - pos[2], pos[1], pos[2]]
else:
return [pos[0] + pos[2], pos[1], pos[2]]
def update_limits(self, i, range):
self.limits[i] = range
def override_rail(self, i, rail):
self.rails[i] = rail
def set_position(self, newpos, homing_axes):
for i, rail in enumerate(self.rails):
rail.set_position(newpos)
if i in homing_axes:
self.limits[i] = rail.get_range()
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limits[2] = (1.0, -1.0)
def _home_axis(self, homing_state, axis, rail):
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= 1.5 * (hi.position_endstop - position_min)
else:
forcepos[axis] += 1.5 * (position_max - hi.position_endstop)
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def home(self, homing_state):
for axis in homing_state.get_axes():
if (self.dc_module is not None and axis == 0):
self.dc_module.save_idex_state()
for i in [0,1]:
self.dc_module.toggle_active_dc_rail(i)
self._home_axis(homing_state, axis, self.rails[0])
self.dc_module.restore_idex_state()
else:
self._home_axis(homing_state, axis, self.rails[axis])
def _motor_off(self, print_time):
self.limits = [(1.0, -1.0)] * 3
def _check_endstops(self, move):
end_pos = move.end_pos
for i in (0, 1, 2):
if (move.axes_d[i]
and (end_pos[i] < self.limits[i][0]
or end_pos[i] > self.limits[i][1])):
if self.limits[i][0] > self.limits[i][1]:
raise move.move_error("Must home axis first")
raise move.move_error()
def check_move(self, move):
limits = self.limits
xpos, ypos = move.end_pos[:2]
if (xpos < limits[0][0] or xpos > limits[0][1]
or ypos < limits[1][0] or ypos > limits[1][1]):
self._check_endstops(move)
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
self._check_endstops(move)
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(
self.max_z_velocity * z_ratio, self.max_z_accel * z_ratio)
def get_status(self, eventtime):
axes = [a for a, (l, h) in zip("xyz", self.limits) if l <= h]
return {
'homed_axes': "".join(axes),
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max
}
def load_kinematics(toolhead, config):
return HybridCoreXZKinematics(toolhead, config)

105
klippy/kinematics/idex_modes.py Normal file
View File

@@ -0,0 +1,105 @@
# Support for duplication and mirroring modes for IDEX printers
#
# Copyright (C) 2021 Fabrice Gallet <tircown@gmail.com>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math
class DualCarriages:
def __init__(self, printer, rail_0, rail_1, axis):
self.printer = printer
self.axis = axis
self.dc = (rail_0, rail_1)
self.saved_state = None
self.printer.add_object('dual_carriage', self)
gcode = self.printer.lookup_object('gcode')
gcode.register_command(
'SET_DUAL_CARRIAGE', self.cmd_SET_DUAL_CARRIAGE,
desc=self.cmd_SET_DUAL_CARRIAGE_help)
def toggle_active_dc_rail(self, index):
toolhead = self.printer.lookup_object('toolhead')
toolhead.flush_step_generation()
pos = toolhead.get_position()
kin = toolhead.get_kinematics()
for i, dc in enumerate(self.dc):
dc_rail = dc.get_rail()
if i != index:
dc.inactivate(pos)
kin.override_rail(3, dc_rail)
elif dc.is_active() is False:
newpos = pos[:self.axis] + [dc.axis_position] \
+ pos[self.axis+1:]
dc.activate(newpos)
kin.override_rail(self.axis, dc_rail)
toolhead.set_position(newpos)
kin.update_limits(self.axis, dc_rail.get_range())
def get_status(self, eventtime=None):
dc0, dc1 = self.dc
if (dc0.is_active() is True):
return { 'mode': 'FULL_CONTROL', 'active_carriage': 'CARRIAGE_0' }
else:
return { 'mode': 'FULL_CONTROL', 'active_carriage': 'CARRIAGE_1' }
def save_idex_state(self):
dc0, dc1 = self.dc
if (dc0.is_active() is True):
mode, active_carriage = ('FULL_CONTROL', 'CARRIAGE_0')
else:
mode, active_carriage = ('FULL_CONTROL', 'CARRIAGE_1')
self.saved_state = {
'mode': mode,
'active_carriage': active_carriage,
'axis_positions': (dc0.axis_position, dc1.axis_position)
}
def restore_idex_state(self):
if self.saved_state is not None:
# set carriage 0 active
if (self.saved_state['active_carriage'] == 'CARRIAGE_0'
and self.dc[0].is_active() is False):
self.toggle_active_dc_rail(0)
# set carriage 1 active
elif (self.saved_state['active_carriage'] == 'CARRIAGE_1'
and self.dc[1].is_active() is False):
self.toggle_active_dc_rail(1)
cmd_SET_DUAL_CARRIAGE_help = "Set which carriage is active"
def cmd_SET_DUAL_CARRIAGE(self, gcmd):
index = gcmd.get_int('CARRIAGE', minval=0, maxval=1)
if (not(self.dc[0].is_active() == self.dc[1].is_active() == True)
and self.dc[index].is_active() is False):
self.toggle_active_dc_rail(index)
class DualCarriagesRail:
ACTIVE=1
INACTIVE=2
def __init__(self, printer, rail, axis, active, stepper_alloc_active,
stepper_alloc_inactive=None):
self.printer = printer
self.rail = rail
self.axis = axis
self.status = (self.INACTIVE, self.ACTIVE)[active]
self.stepper_alloc_active = stepper_alloc_active
self.stepper_alloc_inactive = stepper_alloc_inactive
self.axis_position = -1
def _stepper_alloc(self, position, active=True):
toolhead = self.printer.lookup_object('toolhead')
self.axis_position = position[self.axis]
self.rail.set_trapq(None)
if active is True:
self.status = self.ACTIVE
if self.stepper_alloc_active is not None:
self.rail.setup_itersolve(*self.stepper_alloc_active)
self.rail.set_position(position)
self.rail.set_trapq(toolhead.get_trapq())
else:
self.status = self.INACTIVE
if self.stepper_alloc_inactive is not None:
self.rail.setup_itersolve(*self.stepper_alloc_inactive)
self.rail.set_position(position)
self.rail.set_trapq(toolhead.get_trapq())
def get_rail(self):
return self.rail
def is_active(self):
return self.status == self.ACTIVE
def activate(self, position):
self._stepper_alloc(position, active=True)
def inactivate(self, position):
self._stepper_alloc(position, active=False)

28
klippy/kinematics/none.py Normal file
View File

@@ -0,0 +1,28 @@
# Dummy "none" kinematics support (for developer testing)
#
# Copyright (C) 2018-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
class NoneKinematics:
def __init__(self, toolhead, config):
self.axes_minmax = toolhead.Coord(0., 0., 0., 0.)
def get_steppers(self):
return []
def calc_position(self, stepper_positions):
return [0, 0, 0]
def set_position(self, newpos, homing_axes):
pass
def home(self, homing_state):
pass
def check_move(self, move):
pass
def get_status(self, eventtime):
return {
'homed_axes': '',
'axis_minimum': self.axes_minmax,
'axis_maximum': self.axes_minmax,
}
def load_kinematics(toolhead, config):
return NoneKinematics(toolhead, config)

BIN
klippy/kinematics/none.pyc Normal file
View File

Binary file not shown.

117
klippy/kinematics/polar.py Normal file
View File

@@ -0,0 +1,117 @@
# Code for handling the kinematics of polar robots
#
# Copyright (C) 2018-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import logging, math
import stepper
class PolarKinematics:
def __init__(self, toolhead, config):
# Setup axis steppers
stepper_bed = stepper.PrinterStepper(config.getsection('stepper_bed'),
units_in_radians=True)
rail_arm = stepper.PrinterRail(config.getsection('stepper_arm'))
rail_z = stepper.LookupMultiRail(config.getsection('stepper_z'))
stepper_bed.setup_itersolve('polar_stepper_alloc', b'a')
rail_arm.setup_itersolve('polar_stepper_alloc', b'r')
rail_z.setup_itersolve('cartesian_stepper_alloc', b'z')
self.rails = [rail_arm, rail_z]
self.steppers = [stepper_bed] + [ s for r in self.rails
for s in r.get_steppers() ]
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
config.get_printer().register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Setup boundary checks
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat(
'max_z_velocity', max_velocity, above=0., maxval=max_velocity)
self.max_z_accel = config.getfloat(
'max_z_accel', max_accel, above=0., maxval=max_accel)
self.limit_z = (1.0, -1.0)
self.limit_xy2 = -1.
max_xy = self.rails[0].get_range()[1]
min_z, max_z = self.rails[1].get_range()
self.axes_min = toolhead.Coord(-max_xy, -max_xy, min_z, 0.)
self.axes_max = toolhead.Coord(max_xy, max_xy, max_z, 0.)
def get_steppers(self):
return list(self.steppers)
def calc_position(self, stepper_positions):
bed_angle = stepper_positions[self.steppers[0].get_name()]
arm_pos = stepper_positions[self.rails[0].get_name()]
z_pos = stepper_positions[self.rails[1].get_name()]
return [math.cos(bed_angle) * arm_pos, math.sin(bed_angle) * arm_pos,
z_pos]
def set_position(self, newpos, homing_axes):
for s in self.steppers:
s.set_position(newpos)
if 2 in homing_axes:
self.limit_z = self.rails[1].get_range()
if 0 in homing_axes and 1 in homing_axes:
self.limit_xy2 = self.rails[0].get_range()[1]**2
def note_z_not_homed(self):
# Helper for Safe Z Home
self.limit_z = (1.0, -1.0)
def _home_axis(self, homing_state, axis, rail):
# Determine movement
position_min, position_max = rail.get_range()
hi = rail.get_homing_info()
homepos = [None, None, None, None]
homepos[axis] = hi.position_endstop
if axis == 0:
homepos[1] = 0.
forcepos = list(homepos)
if hi.positive_dir:
forcepos[axis] -= hi.position_endstop - position_min
else:
forcepos[axis] += position_max - hi.position_endstop
# Perform homing
homing_state.home_rails([rail], forcepos, homepos)
def home(self, homing_state):
# Always home XY together
homing_axes = homing_state.get_axes()
home_xy = 0 in homing_axes or 1 in homing_axes
home_z = 2 in homing_axes
updated_axes = []
if home_xy:
updated_axes = [0, 1]
if home_z:
updated_axes.append(2)
homing_state.set_axes(updated_axes)
# Do actual homing
if home_xy:
self._home_axis(homing_state, 0, self.rails[0])
if home_z:
self._home_axis(homing_state, 2, self.rails[1])
def _motor_off(self, print_time):
self.limit_z = (1.0, -1.0)
self.limit_xy2 = -1.
def check_move(self, move):
end_pos = move.end_pos
xy2 = end_pos[0]**2 + end_pos[1]**2
if xy2 > self.limit_xy2:
if self.limit_xy2 < 0.:
raise move.move_error("Must home axis first")
raise move.move_error()
if move.axes_d[2]:
if end_pos[2] < self.limit_z[0] or end_pos[2] > self.limit_z[1]:
if self.limit_z[0] > self.limit_z[1]:
raise move.move_error("Must home axis first")
raise move.move_error()
# Move with Z - update velocity and accel for slower Z axis
z_ratio = move.move_d / abs(move.axes_d[2])
move.limit_speed(self.max_z_velocity * z_ratio,
self.max_z_accel * z_ratio)
def get_status(self, eventtime):
xy_home = "xy" if self.limit_xy2 >= 0. else ""
z_home = "z" if self.limit_z[0] <= self.limit_z[1] else ""
return {
'homed_axes': xy_home + z_home,
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def load_kinematics(toolhead, config):
return PolarKinematics(toolhead, config)

228
klippy/kinematics/rotary_delta.py Normal file
View File

@@ -0,0 +1,228 @@
# Code for handling the kinematics of rotary delta robots
#
# Copyright (C) 2019-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging
import stepper, mathutil, chelper
class RotaryDeltaKinematics:
def __init__(self, toolhead, config):
# Setup tower rails
stepper_configs = [config.getsection('stepper_' + a) for a in 'abc']
rail_a = stepper.PrinterRail(
stepper_configs[0], need_position_minmax=False,
units_in_radians=True)
a_endstop = rail_a.get_homing_info().position_endstop
rail_b = stepper.PrinterRail(
stepper_configs[1], need_position_minmax=False,
default_position_endstop=a_endstop, units_in_radians=True)
rail_c = stepper.PrinterRail(
stepper_configs[2], need_position_minmax=False,
default_position_endstop=a_endstop, units_in_radians=True)
self.rails = [rail_a, rail_b, rail_c]
config.get_printer().register_event_handler("stepper_enable:motor_off",
self._motor_off)
# Read config
max_velocity, max_accel = toolhead.get_max_velocity()
self.max_z_velocity = config.getfloat('max_z_velocity', max_velocity,
above=0., maxval=max_velocity)
shoulder_radius = config.getfloat('shoulder_radius', above=0.)
shoulder_height = config.getfloat('shoulder_height', above=0.)
a_upper_arm = stepper_configs[0].getfloat('upper_arm_length', above=0.)
upper_arms = [
sconfig.getfloat('upper_arm_length', a_upper_arm, above=0.)
for sconfig in stepper_configs]
a_lower_arm = stepper_configs[0].getfloat('lower_arm_length', above=0.)
lower_arms = [
sconfig.getfloat('lower_arm_length', a_lower_arm, above=0.)
for sconfig in stepper_configs]
angles = [sconfig.getfloat('angle', angle)
for sconfig, angle in zip(stepper_configs, [30., 150., 270.])]
# Setup rotary delta calibration helper
endstops = [rail.get_homing_info().position_endstop
for rail in self.rails]
stepdists = [rail.get_steppers()[0].get_step_dist()
for rail in self.rails]
self.calibration = RotaryDeltaCalibration(
shoulder_radius, shoulder_height, angles, upper_arms, lower_arms,
endstops, stepdists)
# Setup iterative solver
for r, a, ua, la in zip(self.rails, angles, upper_arms, lower_arms):
r.setup_itersolve('rotary_delta_stepper_alloc',
shoulder_radius, shoulder_height,
math.radians(a), ua, la)
for s in self.get_steppers():
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
# Setup boundary checks
self.need_home = True
self.limit_xy2 = -1.
eangles = [r.calc_position_from_coord([0., 0., ep])
for r, ep in zip(self.rails, endstops)]
self.home_position = tuple(
self.calibration.actuator_to_cartesian(eangles))
self.max_z = min(endstops)
self.min_z = config.getfloat('minimum_z_position', 0, maxval=self.max_z)
min_ua = min([shoulder_radius + ua for ua in upper_arms])
min_la = min([la - shoulder_radius for la in lower_arms])
self.max_xy2 = min(min_ua, min_la)**2
arm_z = [self.calibration.elbow_coord(i, ea)[2]
for i, ea in enumerate(eangles)]
self.limit_z = min([az - la for az, la in zip(arm_z, lower_arms)])
logging.info(
"Delta max build height %.2fmm (radius tapered above %.2fmm)"
% (self.max_z, self.limit_z))
max_xy = math.sqrt(self.max_xy2)
self.axes_min = toolhead.Coord(-max_xy, -max_xy, self.min_z, 0.)
self.axes_max = toolhead.Coord(max_xy, max_xy, self.max_z, 0.)
self.set_position([0., 0., 0.], ())
def get_steppers(self):
return [s for rail in self.rails for s in rail.get_steppers()]
def calc_position(self, stepper_positions):
spos = [stepper_positions[rail.get_name()] for rail in self.rails]
return self.calibration.actuator_to_cartesian(spos)
def set_position(self, newpos, homing_axes):
for rail in self.rails:
rail.set_position(newpos)
self.limit_xy2 = -1.
if tuple(homing_axes) == (0, 1, 2):
self.need_home = False
def home(self, homing_state):
# All axes are homed simultaneously
homing_state.set_axes([0, 1, 2])
forcepos = list(self.home_position)
#min_angles = [-.5 * math.pi] * 3
#forcepos[2] = self.calibration.actuator_to_cartesian(min_angles)[2]
forcepos[2] = -1.
homing_state.home_rails(self.rails, forcepos, self.home_position)
def _motor_off(self, print_time):
self.limit_xy2 = -1.
self.need_home = True
def check_move(self, move):
end_pos = move.end_pos
end_xy2 = end_pos[0]**2 + end_pos[1]**2
if end_xy2 <= self.limit_xy2 and not move.axes_d[2]:
# Normal XY move
return
if self.need_home:
raise move.move_error("Must home first")
end_z = end_pos[2]
limit_xy2 = self.max_xy2
if end_z > self.limit_z:
limit_xy2 = min(limit_xy2, (self.max_z - end_z)**2)
if end_xy2 > limit_xy2 or end_z > self.max_z or end_z < self.min_z:
# Move out of range - verify not a homing move
if (end_pos[:2] != self.home_position[:2]
or end_z < self.min_z or end_z > self.home_position[2]):
raise move.move_error()
limit_xy2 = -1.
if move.axes_d[2]:
move.limit_speed(self.max_z_velocity, move.accel)
limit_xy2 = -1.
self.limit_xy2 = limit_xy2
def get_status(self, eventtime):
return {
'homed_axes': '' if self.need_home else 'xyz',
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def get_calibration(self):
return self.calibration
# Rotary delta parameter calibration for DELTA_CALIBRATE tool
class RotaryDeltaCalibration:
def __init__(self, shoulder_radius, shoulder_height, angles,
upper_arms, lower_arms, endstops, stepdists):
self.shoulder_radius = shoulder_radius
self.shoulder_height = shoulder_height
self.angles = angles
self.upper_arms = upper_arms
self.lower_arms = lower_arms
self.endstops = endstops
self.stepdists = stepdists
# Calculate the absolute angle of each endstop
ffi_main, self.ffi_lib = chelper.get_ffi()
self.sks = [ffi_main.gc(self.ffi_lib.rotary_delta_stepper_alloc(
shoulder_radius, shoulder_height, math.radians(a), ua, la),
self.ffi_lib.free)
for a, ua, la in zip(angles, upper_arms, lower_arms)]
self.abs_endstops = [
self.ffi_lib.itersolve_calc_position_from_coord(sk, 0., 0., es)
for sk, es in zip(self.sks, endstops)]
def coordinate_descent_params(self, is_extended):
# Determine adjustment parameters (for use with coordinate_descent)
adj_params = ('shoulder_height', 'endstop_a', 'endstop_b', 'endstop_c')
if is_extended:
adj_params += ('shoulder_radius', 'angle_a', 'angle_b')
params = { 'shoulder_radius': self.shoulder_radius,
'shoulder_height': self.shoulder_height }
for i, axis in enumerate('abc'):
params['angle_'+axis] = self.angles[i]
params['upper_arm_'+axis] = self.upper_arms[i]
params['lower_arm_'+axis] = self.lower_arms[i]
params['endstop_'+axis] = self.endstops[i]
params['stepdist_'+axis] = self.stepdists[i]
return adj_params, params
def new_calibration(self, params):
# Create a new calibration object from coordinate_descent params
shoulder_radius = params['shoulder_radius']
shoulder_height = params['shoulder_height']
angles = [params['angle_'+a] for a in 'abc']
upper_arms = [params['upper_arm_'+a] for a in 'abc']
lower_arms = [params['lower_arm_'+a] for a in 'abc']
endstops = [params['endstop_'+a] for a in 'abc']
stepdists = [params['stepdist_'+a] for a in 'abc']
return RotaryDeltaCalibration(
shoulder_radius, shoulder_height, angles, upper_arms, lower_arms,
endstops, stepdists)
def elbow_coord(self, elbow_id, spos):
# Calculate elbow position in coordinate system at shoulder joint
sj_elbow_x = self.upper_arms[elbow_id] * math.cos(spos)
sj_elbow_y = self.upper_arms[elbow_id] * math.sin(spos)
# Shift and rotate to main cartesian coordinate system
angle = math.radians(self.angles[elbow_id])
x = (sj_elbow_x + self.shoulder_radius) * math.cos(angle)
y = (sj_elbow_x + self.shoulder_radius) * math.sin(angle)
z = sj_elbow_y + self.shoulder_height
return (x, y, z)
def actuator_to_cartesian(self, spos):
sphere_coords = [self.elbow_coord(i, sp) for i, sp in enumerate(spos)]
lower_arm2 = [la**2 for la in self.lower_arms]
return mathutil.trilateration(sphere_coords, lower_arm2)
def get_position_from_stable(self, stable_position):
# Return cartesian coordinates for the given stable_position
spos = [ea - sp * sd
for ea, sp, sd in zip(self.abs_endstops, stable_position,
self.stepdists)]
return self.actuator_to_cartesian(spos)
def calc_stable_position(self, coord):
# Return a stable_position from a cartesian coordinate
pos = [ self.ffi_lib.itersolve_calc_position_from_coord(
sk, coord[0], coord[1], coord[2])
for sk in self.sks ]
return [(ep - sp) / sd
for sd, ep, sp in zip(self.stepdists, self.abs_endstops, pos)]
def save_state(self, configfile):
# Save the current parameters (for use with SAVE_CONFIG)
configfile.set('printer', 'shoulder_radius', "%.6f"
% (self.shoulder_radius,))
configfile.set('printer', 'shoulder_height', "%.6f"
% (self.shoulder_height,))
for i, axis in enumerate('abc'):
configfile.set('stepper_'+axis, 'angle', "%.6f" % (self.angles[i],))
configfile.set('stepper_'+axis, 'position_endstop',
"%.6f" % (self.endstops[i],))
gcode = configfile.get_printer().lookup_object("gcode")
gcode.respond_info(
"stepper_a: position_endstop: %.6f angle: %.6f\n"
"stepper_b: position_endstop: %.6f angle: %.6f\n"
"stepper_c: position_endstop: %.6f angle: %.6f\n"
"shoulder_radius: %.6f shoulder_height: %.6f"
% (self.endstops[0], self.angles[0],
self.endstops[1], self.angles[1],
self.endstops[2], self.angles[2],
self.shoulder_radius, self.shoulder_height))
def load_kinematics(toolhead, config):
return RotaryDeltaKinematics(toolhead, config)

55
klippy/kinematics/winch.py Normal file
View File

@@ -0,0 +1,55 @@
# Code for handling the kinematics of cable winch robots
#
# Copyright (C) 2018-2021 Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import stepper, mathutil
class WinchKinematics:
def __init__(self, toolhead, config):
# Setup steppers at each anchor
self.steppers = []
self.anchors = []
for i in range(26):
name = 'stepper_' + chr(ord('a') + i)
if i >= 3 and not config.has_section(name):
break
stepper_config = config.getsection(name)
s = stepper.PrinterStepper(stepper_config)
self.steppers.append(s)
a = tuple([stepper_config.getfloat('anchor_' + n) for n in 'xyz'])
self.anchors.append(a)
s.setup_itersolve('winch_stepper_alloc', *a)
s.set_trapq(toolhead.get_trapq())
toolhead.register_step_generator(s.generate_steps)
# Setup boundary checks
acoords = list(zip(*self.anchors))
self.axes_min = toolhead.Coord(*[min(a) for a in acoords], e=0.)
self.axes_max = toolhead.Coord(*[max(a) for a in acoords], e=0.)
self.set_position([0., 0., 0.], ())
def get_steppers(self):
return list(self.steppers)
def calc_position(self, stepper_positions):
# Use only first three steppers to calculate cartesian position
pos = [stepper_positions[rail.get_name()] for rail in self.steppers[:3]]
return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in pos])
def set_position(self, newpos, homing_axes):
for s in self.steppers:
s.set_position(newpos)
def home(self, homing_state):
# XXX - homing not implemented
homing_state.set_axes([0, 1, 2])
homing_state.set_homed_position([0., 0., 0.])
def check_move(self, move):
# XXX - boundary checks and speed limits not implemented
pass
def get_status(self, eventtime):
# XXX - homed_checks and rail limits not implemented
return {
'homed_axes': 'xyz',
'axis_minimum': self.axes_min,
'axis_maximum': self.axes_max,
}
def load_kinematics(toolhead, config):
return WinchKinematics(toolhead, config)