diff options
Diffstat (limited to 'klippy/kinematics/generic_cartesian.py')
| -rw-r--r-- | klippy/kinematics/generic_cartesian.py | 227 |
1 files changed, 154 insertions, 73 deletions
diff --git a/klippy/kinematics/generic_cartesian.py b/klippy/kinematics/generic_cartesian.py index b8cabb77..a76e4a21 100644 --- a/klippy/kinematics/generic_cartesian.py +++ b/klippy/kinematics/generic_cartesian.py @@ -9,6 +9,7 @@ import gcode, mathutil, stepper from . import idex_modes from . import kinematic_stepper as ks + def mat_mul(a, b): if len(a[0]) != len(b): return None @@ -19,89 +20,118 @@ def mat_mul(a, b): res[i].append(sum(a[i][k] * b[k][j] for k in range(len(b)))) return res + def mat_transp(a): res = [] for i in range(len(a[0])): res.append([a[j][i] for j in range(len(a))]) return res + def mat_pseudo_inverse(m): mt = mat_transp(m) mtm = mat_mul(mt, m) pinv = mat_mul(mathutil.matrix_inv(mtm), mt) return pinv + class MainCarriage: def __init__(self, config, axis): self.rail = stepper.GenericPrinterRail(config) - self.axis = ord(axis) - ord('x') + self.axis = ord(axis) - ord("x") self.axis_name = axis self.dual_carriage = None + def get_name(self): return self.rail.get_name(short=True) + def get_axis(self): return self.axis + def get_rail(self): return self.rail + def add_stepper(self, kin_stepper): self.rail.add_stepper(kin_stepper.get_stepper()) + def is_active(self): if self.dual_carriage is None: return True return self.dual_carriage.get_dc_module().is_active(self.rail) + def set_dual_carriage(self, carriage): old_dc = self.dual_carriage self.dual_carriage = carriage return old_dc + def get_dual_carriage(self): return self.dual_carriage + class ExtraCarriage: def __init__(self, config, carriages): self.name = config.get_name().split()[-1] - self.primary_carriage = config.getchoice('primary_carriage', carriages) - self.endstop_pin = config.get('endstop_pin') + self.primary_carriage = config.getchoice("primary_carriage", carriages) + self.endstop_pin = config.get("endstop_pin") + def get_name(self): return self.name + def get_axis(self): return self.primary_carriage.get_axis() + def get_rail(self): return self.primary_carriage.get_rail() + def add_stepper(self, kin_stepper): - self.get_rail().add_stepper(kin_stepper.get_stepper(), - self.endstop_pin, self.name) + self.get_rail().add_stepper( + kin_stepper.get_stepper(), self.endstop_pin, self.name + ) + class DualCarriage: def __init__(self, config, carriages): self.printer = config.get_printer() self.rail = stepper.GenericPrinterRail(config) - self.primary_carriage = config.getchoice('primary_carriage', carriages) + self.primary_carriage = config.getchoice("primary_carriage", carriages) if self.primary_carriage.set_dual_carriage(self) is not None: raise config.error( - "Redefinition of dual_carriage for carriage '%s'" % - self.primary_carriage.get_name()) + "Redefinition of dual_carriage for carriage '%s'" + % self.primary_carriage.get_name() + ) self.axis = self.primary_carriage.get_axis() if self.axis > 1: - raise config.error("Invalid axis '%s' for dual_carriage" % - self.primary_carriage.get_axis_name()) - self.safe_dist = config.getfloat('safe_distance', None, minval=0.) + raise config.error( + "Invalid axis '%s' for dual_carriage" + % self.primary_carriage.get_axis_name() + ) + self.safe_dist = config.getfloat("safe_distance", None, minval=0.0) + def get_name(self): return self.rail.get_name(short=True) + def get_axis(self): return self.primary_carriage.get_axis() + def get_rail(self): return self.rail + def get_safe_dist(self): return self.safe_dist + def get_dc_module(self): - return self.printer.lookup_object('dual_carriage') + return self.printer.lookup_object("dual_carriage") + def is_active(self): return self.get_dc_module().is_active(self.rail) + def get_dual_carriage(self): return self.primary_carriage + def add_stepper(self, kin_stepper): self.rail.add_stepper(kin_stepper.get_stepper()) + class GenericCartesianKinematics: def __init__(self, toolhead, config): self.printer = config.get_printer() @@ -115,42 +145,50 @@ class GenericCartesianKinematics: primary_rails = [pc.get_rail() for pc in pcs] dual_rails = [dc.get_rail() for dc in self.dc_carriages] axes = [dc.get_axis() for dc in self.dc_carriages] - safe_dist = {dc.get_axis() : dc.get_safe_dist() - for dc in self.dc_carriages} + safe_dist = {dc.get_axis(): dc.get_safe_dist() for dc in self.dc_carriages} self.dc_module = dc_module = idex_modes.DualCarriages( - self.printer, primary_rails, dual_rails, axes, safe_dist) - zero_pos = (0., 0.) + self.printer, primary_rails, dual_rails, axes, safe_dist + ) + zero_pos = (0.0, 0.0) for acs in itertools.product(*zip(pcs, self.dc_carriages)): for c in acs: dc_module.get_dc_rail_wrapper(c.get_rail()).activate( - idex_modes.PRIMARY, zero_pos) + idex_modes.PRIMARY, zero_pos + ) dc_rail = c.get_dual_carriage().get_rail() dc_module.get_dc_rail_wrapper(dc_rail).inactivate(zero_pos) self._check_kinematics(config.error) for c in pcs: dc_module.get_dc_rail_wrapper(c.get_rail()).activate( - idex_modes.PRIMARY, zero_pos) + idex_modes.PRIMARY, zero_pos + ) dc_rail = c.get_dual_carriage().get_rail() dc_module.get_dc_rail_wrapper(dc_rail).inactivate(zero_pos) else: self._check_kinematics(config.error) # 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.max_z_velocity = config.getfloat( + "max_z_velocity", max_velocity, above=0.0, maxval=max_velocity + ) + self.max_z_accel = config.getfloat( + "max_z_accel", max_accel, above=0.0, maxval=max_accel + ) self.limits = [(1.0, -1.0)] * 3 # Register gcode commands - gcode = self.printer.lookup_object('gcode') - gcode.register_command("SET_STEPPER_CARRIAGES", - self.cmd_SET_STEPPER_CARRIAGES, - desc=self.cmd_SET_STEPPER_CARRIAGES_help) + gcode = self.printer.lookup_object("gcode") + gcode.register_command( + "SET_STEPPER_CARRIAGES", + self.cmd_SET_STEPPER_CARRIAGES, + desc=self.cmd_SET_STEPPER_CARRIAGES_help, + ) + def _load_kinematics(self, config): - carriages = {a : MainCarriage(config.getsection('carriage ' + a), a) - for a in 'xyz'} + carriages = { + a: MainCarriage(config.getsection("carriage " + a), a) for a in "xyz" + } dc_carriages = [] - for c in config.get_prefix_sections('dual_carriage '): + for c in config.get_prefix_sections("dual_carriage "): dc_carriages.append(DualCarriage(c, carriages)) for dc in dc_carriages: name = dc.get_name() @@ -160,7 +198,7 @@ class GenericCartesianKinematics: self.carriages = dict(carriages) self.dc_carriages = dc_carriages ec_carriages = [] - for c in config.get_prefix_sections('extra_carriage '): + for c in config.get_prefix_sections("extra_carriage "): ec_carriages.append(ExtraCarriage(c, carriages)) for ec in ec_carriages: name = ec.get_name() @@ -171,6 +209,7 @@ class GenericCartesianKinematics: self.all_carriages = carriages self._check_carriages_references(config.error) self._check_multi_mcu_homing(config.error) + def _check_carriages_references(self, report_error): carriages = dict(self.all_carriages) for s in self.kin_steppers: @@ -178,21 +217,35 @@ class GenericCartesianKinematics: carriages.pop(c.get_name(), None) if carriages: raise report_error( - "Carriage(s) %s must be referenced by some " - "stepper(s)" % (", ".join(carriages),)) + "Carriage(s) %s must be referenced by some " + "stepper(s)" % (", ".join(carriages),) + ) + def _check_multi_mcu_homing(self, report_error): for carriage in self.carriages.values(): for es in carriage.get_rail().get_endstops(): - stepper_mcus = set([s.get_mcu() for s in es[0].get_steppers() - if s in carriage.get_rail().get_steppers()]) + stepper_mcus = set( + [ + s.get_mcu() + for s in es[0].get_steppers() + if s in carriage.get_rail().get_steppers() + ] + ) if len(stepper_mcus) > 1: - raise report_error("Multi-mcu homing not supported on" - " multi-mcu shared carriage %s" % es[1]) + raise report_error( + "Multi-mcu homing not supported on" + " multi-mcu shared carriage %s" % es[1] + ) + def _load_steppers(self, config, carriages): - return [ks.KinematicStepper(c, carriages) - for c in config.get_prefix_sections('stepper ')] + return [ + ks.KinematicStepper(c, carriages) + for c in config.get_prefix_sections("stepper ") + ] + def get_steppers(self): return [s.get_stepper() for s in self.kin_steppers] + def get_primary_carriages(self): carriages = [self.carriages[a] for a in "xyz"] if self.dc_module: @@ -202,13 +255,15 @@ class GenericCartesianKinematics: if c.get_rail() == primary_rail: carriages[a] = c return carriages + def _get_kinematics_coeffs(self): - matr = {s.get_name() : list(s.get_kin_coeffs()) - for s in self.kin_steppers} - offs = {s.get_name() : [0.] * 3 for s in self.kin_steppers} + matr = {s.get_name(): list(s.get_kin_coeffs()) for s in self.kin_steppers} + offs = {s.get_name(): [0.0] * 3 for s in self.kin_steppers} if self.dc_module is None: - return ([matr[s.get_name()] for s in self.kin_steppers], - [0. for s in self.kin_steppers]) + return ( + [matr[s.get_name()] for s in self.kin_steppers], + [0.0 for s in self.kin_steppers], + ) axes = [dc.get_axis() for dc in self.dc_carriages] orig_matr = copy.deepcopy(matr) for dc in self.dc_carriages: @@ -218,33 +273,41 @@ class GenericCartesianKinematics: for s in c.get_rail().get_steppers(): matr[s.get_name()][axis] *= m offs[s.get_name()][axis] += o - return ([matr[s.get_name()] for s in self.kin_steppers], - [mathutil.matrix_dot(orig_matr[s.get_name()], - offs[s.get_name()]) - for s in self.kin_steppers]) + return ( + [matr[s.get_name()] for s in self.kin_steppers], + [ + mathutil.matrix_dot(orig_matr[s.get_name()], offs[s.get_name()]) + for s in self.kin_steppers + ], + ) + def _check_kinematics(self, report_error): matr, _ = self._get_kinematics_coeffs() det = mathutil.matrix_det(mat_mul(mat_transp(matr), matr)) if abs(det) < 0.00001: raise report_error( - "Verify configured stepper(s) and their 'carriages' " - "specifications, the current configuration does not " - "allow independent movements of all printer axes.") + "Verify configured stepper(s) and their 'carriages' " + "specifications, the current configuration does not " + "allow independent movements of all printer axes." + ) + def calc_position(self, stepper_positions): matr, offs = self._get_kinematics_coeffs() spos = [stepper_positions[s.get_name()] for s in self.kin_steppers] pinv = mat_pseudo_inverse(matr) - pos = mat_mul([[sp-o for sp, o in zip(spos, offs)]], mat_transp(pinv)) + pos = mat_mul([[sp - o for sp, o in zip(spos, offs)]], mat_transp(pinv)) for i in range(3): if not any(pinv[i]): pos[0][i] = None return pos[0] + def update_limits(self, i, range): l, h = self.limits[i] # Only update limits if this axis was already homed, # otherwise leave in un-homed state. if l <= h: self.limits[i] = range + def set_position(self, newpos, homing_axes): for s in self.kin_steppers: s.set_position(newpos) @@ -254,10 +317,12 @@ class GenericCartesianKinematics: if c.get_axis() == axis and c.is_active(): self.limits[axis] = c.get_rail().get_range() break + def clear_homing_state(self, clear_axes): for axis, axis_name in enumerate("xyz"): if axis_name in clear_axes: self.limits[axis] = (1.0, -1.0) + def home_axis(self, homing_state, axis, rail): # Determine movement position_min, position_max = rail.get_range() @@ -271,6 +336,7 @@ class GenericCartesianKinematics: forcepos[axis] += 1.5 * (position_max - hi.position_endstop) # Perform homing homing_state.home_rails([rail], forcepos, homepos) + def home(self, homing_state): self._check_kinematics(self.printer.command_error) # Each axis is homed independently and in order @@ -280,20 +346,26 @@ class GenericCartesianKinematics: self.dc_module.home(homing_state, axis) else: self.home_axis(homing_state, axis, carriage.get_rail()) + 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 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]): + 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 @@ -301,27 +373,31 @@ class GenericCartesianKinematics: # 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) + 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] - ranges = [c.get_rail().get_range() - for c in self.get_primary_carriages()] - axes_min = gcode.Coord(*[r[0] for r in ranges], e=0.) - axes_max = gcode.Coord(*[r[1] for r in ranges], e=0.) + ranges = [c.get_rail().get_range() for c in self.get_primary_carriages()] + axes_min = gcode.Coord(*[r[0] for r in ranges], e=0.0) + axes_max = gcode.Coord(*[r[1] for r in ranges], e=0.0) return { - 'homed_axes': "".join(axes), - 'axis_minimum': axes_min, - 'axis_maximum': axes_max, + "homed_axes": "".join(axes), + "axis_minimum": axes_min, + "axis_maximum": axes_max, } + cmd_SET_STEPPER_CARRIAGES_help = "Set stepper carriages" + def cmd_SET_STEPPER_CARRIAGES(self, gcmd): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() stepper_name = gcmd.get("STEPPER") - steppers = [stepper for stepper in self.kin_steppers - if stepper.get_name() == stepper_name - or stepper.get_name(short=True) == stepper_name] + steppers = [ + stepper + for stepper in self.kin_steppers + if stepper.get_name() == stepper_name + or stepper.get_name(short=True) == stepper_name + ] if len(steppers) != 1: raise gcmd.error("Invalid STEPPER '%s' specified" % stepper_name) stepper = steppers[0] @@ -333,8 +409,10 @@ class GenericCartesianKinematics: new_carriages = stepper.get_carriages() if new_carriages != old_carriages: stepper.update_kin_coeffs(old_kin_coeffs) - raise gcmd.error("SET_STEPPER_CARRIAGES cannot add or remove " - "carriages that the stepper controls") + raise gcmd.error( + "SET_STEPPER_CARRIAGES cannot add or remove " + "carriages that the stepper controls" + ) pos = toolhead.get_position() stepper.set_position(pos) if not validate: @@ -346,13 +424,16 @@ class GenericCartesianKinematics: for acs in itertools.product(*zip(pcs, self.dc_carriages)): for c in acs: self.dc_module.get_dc_rail_wrapper(c.get_rail()).activate( - idex_modes.PRIMARY, pos) + idex_modes.PRIMARY, pos + ) self.dc_module.get_dc_rail_wrapper( - c.get_dual_carriage().get_rail()).inactivate(pos) + c.get_dual_carriage().get_rail() + ).inactivate(pos) self._check_kinematics(gcmd.error) self.dc_module.restore_dual_carriage_state(dc_state, move=0) else: self._check_kinematics(gcmd.error) + def load_kinematics(toolhead, config): return GenericCartesianKinematics(toolhead, config) |