From 307b901579410c08efedc1d2b49e42861bf76080 Mon Sep 17 00:00:00 2001 From: Dmitry Butyugin Date: Tue, 2 Jul 2024 20:41:45 +0200 Subject: [PATCH] kinematics: An intial implementation of generic_cartesian kinematics Signed-off-by: Dmitry Butyugin --- klippy/chelper/__init__.py | 7 +- klippy/chelper/kin_generic.c | 43 ++++++ klippy/extras/endstop_phase.py | 4 +- klippy/extras/kinematic_stepper.py | 103 +++++++++++++ klippy/kinematics/cartesian.py | 22 +-- klippy/kinematics/generic_cartesian.py | 205 +++++++++++++++++++++++++ klippy/kinematics/hybrid_corexy.py | 6 +- klippy/kinematics/hybrid_corexz.py | 6 +- klippy/kinematics/idex_modes.py | 6 +- klippy/mathutil.py | 15 ++ klippy/stepper.py | 120 +++++++++------ 11 files changed, 474 insertions(+), 63 deletions(-) create mode 100644 klippy/chelper/kin_generic.c create mode 100644 klippy/extras/kinematic_stepper.py create mode 100644 klippy/kinematics/generic_cartesian.py diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index e4199561d018..7d0c8f60be51 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -21,7 +21,7 @@ 'pollreactor.c', 'msgblock.c', 'trdispatch.c', 'kin_cartesian.c', 'kin_corexy.c', 'kin_corexz.c', 'kin_delta.c', 'kin_deltesian.c', 'kin_polar.c', 'kin_rotary_delta.c', 'kin_winch.c', - 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', + 'kin_extruder.c', 'kin_shaper.c', 'kin_idex.c', 'kin_generic.c' ] DEST_LIB = "c_helper.so" OTHER_FILES = [ @@ -106,6 +106,10 @@ defs_kin_cartesian = """ struct stepper_kinematics *cartesian_stepper_alloc(char axis); """ +defs_kin_generic_cartesian = """ + struct stepper_kinematics *generic_cartesian_stepper_alloc(double a_x + , double a_y, double a_z); +""" defs_kin_corexy = """ struct stepper_kinematics *corexy_stepper_alloc(char type); @@ -223,6 +227,7 @@ defs_kin_cartesian, defs_kin_corexy, defs_kin_corexz, defs_kin_delta, defs_kin_deltesian, defs_kin_polar, defs_kin_rotary_delta, defs_kin_winch, defs_kin_extruder, defs_kin_shaper, defs_kin_idex, + defs_kin_generic_cartesian, ] # Update filenames to an absolute path diff --git a/klippy/chelper/kin_generic.c b/klippy/chelper/kin_generic.c new file mode 100644 index 000000000000..469d876fd034 --- /dev/null +++ b/klippy/chelper/kin_generic.c @@ -0,0 +1,43 @@ +// Generic cartesian kinematics stepper position calculation +// +// Copyright (C) 2024 Dmitry Butyugin +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // offsetof +#include // malloc +#include // memset +#include "compiler.h" // __visible +#include "itersolve.h" // struct stepper_kinematics +#include "trapq.h" // move_get_coord + +struct generic_cartesian_stepper { + struct stepper_kinematics sk; + struct coord a; +}; + +static double +generic_cartesian_stepper_calc_position(struct stepper_kinematics *sk + , struct move *m, double move_time) +{ + struct generic_cartesian_stepper *cs = container_of( + sk, struct generic_cartesian_stepper, sk); + struct coord c = move_get_coord(m, move_time); + return cs->a.x * c.x + cs->a.y * c.y + cs->a.z * c.z; +} + +struct stepper_kinematics * __visible +generic_cartesian_stepper_alloc(double a_x, double a_y, double a_z) +{ + struct generic_cartesian_stepper *cs = malloc(sizeof(*cs)); + memset(cs, 0, sizeof(*cs)); + cs->a.x = a_x; + cs->a.y = a_y; + cs->a.z = a_z; + cs->sk.calc_position_cb = generic_cartesian_stepper_calc_position; + cs->sk.active_flags = 0; + if (a_x) cs->sk.active_flags |= AF_X; + if (a_y) cs->sk.active_flags |= AF_Y; + if (a_z) cs->sk.active_flags |= AF_Z; + return &cs->sk; +} diff --git a/klippy/extras/endstop_phase.py b/klippy/extras/endstop_phase.py index 2c7468bce3e0..689ec4fafd77 100644 --- a/klippy/extras/endstop_phase.py +++ b/klippy/extras/endstop_phase.py @@ -52,7 +52,7 @@ def calc_phase(self, stepper, trig_mcu_pos): class EndstopPhase: def __init__(self, config): self.printer = config.get_printer() - self.name = config.get_name().split()[1] + self.name = " ".join(config.get_name().split()[1:]) # Obtain step_distance and microsteps from stepper config section sconfig = config.getsection(self.name) rotation_dist, steps_per_rotation = stepper.parse_step_distance(sconfig) @@ -118,7 +118,7 @@ def get_homed_offset(self, stepper, trig_mcu_pos): return delta * self.step_dist def handle_home_rails_end(self, homing_state, rails): for rail in rails: - stepper = rail.get_steppers()[0] + stepper = rail.get_endstops()[0][0].get_steppers()[0] if stepper.get_name() == self.name: trig_mcu_pos = homing_state.get_trigger_position(self.name) align = self.align_endstop(rail) diff --git a/klippy/extras/kinematic_stepper.py b/klippy/extras/kinematic_stepper.py new file mode 100644 index 000000000000..979d5357ccb9 --- /dev/null +++ b/klippy/extras/kinematic_stepper.py @@ -0,0 +1,103 @@ +# Kinematic stepper class for generic cartesian kinematics +# +# Copyright (C) 2024 Dmitry Butyugin +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import re +import stepper, chelper + +def parse_kinematic_string(s, error): + nxt = 0 + pat = re.compile('[+-]') + res = [0.] * 3 + while nxt < len(s): + match = pat.search(s, nxt+1) + end = len(s) if match is None else match.start() + term = s[nxt:end].strip() + term_lst = term.split("*") + if len(term_lst) not in [1, 2]: + raise error("Invalid term '%s' in kinematics '%s'" % (term, s)) + if len(term_lst) == 2: + try: + coeff = float(term_lst[0]) + except ValueError: + raise error("Invalid float '%s'" % term_lst[0]) + else: + coeff = -1. if term_lst[0].startswith('-') else 1. + jj = [i for i, a in enumerate("xyz") + if term_lst[-1] in ['-' + a, '+' + a, a]] + if len(jj) != 1: + raise error("Invalid term '%s' in kinematics '%s'" % (term, s)) + if res[jj[0]]: + raise error("Axis '%s' was specified multiple times in " + "kinematics '%s'" % ("xyz"[i], s)) + res[jj[0]] = coeff + nxt = end + return res + +class KinematicStepper: + def __init__(self, config): + self.printer = config.get_printer() + self.config = config + self.stepper = stepper.PrinterStepper(config) + kinematics_str = config.get('kinematics') + self.kin_coeffs = parse_kinematic_string(kinematics_str, config.error) + if not any(self.kin_coeffs): + raise config.error( + "'%s' must provide a valid 'kinematics' configuration" % + self.stepper.get_name()) + self.carriage = self.config.get('carriage', None) + self.endstop_pin = self.config.get('endstop_pin', None) + if self.endstop_pin is not None and self.carriage is None and \ + len(self.get_active_axes()) > 1: + raise config.error( + "kinematics '%s' for '%s' requires that 'carriage' " + "is set in order to use 'endstop_pin'" % ( + kinematics_str, self.stepper.get_name())) + # TODO: Add shortcuts for some typical optimized kinematics + self.stepper.setup_itersolve( + 'generic_cartesian_stepper_alloc', + self.kin_coeffs[0], self.kin_coeffs[1], self.kin_coeffs[2]) + self.get_name = self.stepper.get_name + self.get_step_dist = self.stepper.get_step_dist + self.units_in_radians = self.stepper.units_in_radians + self.get_pulse_duration = self.stepper.get_pulse_duration + self.setup_default_pulse_duration = \ + self.stepper.setup_default_pulse_duration + self.get_mcu = self.stepper.get_mcu + self.get_oid = self.stepper.get_oid + self.get_trapq = self.stepper.get_trapq + self.set_trapq = self.stepper.set_trapq + self.get_dir_inverted = self.stepper.get_dir_inverted + self.set_dir_inverted = self.stepper.set_dir_inverted + self.set_position = self.stepper.set_position + self.get_commanded_position = self.stepper.get_commanded_position + self.get_mcu_position = self.stepper.get_mcu_position + self.get_past_mcu_position = self.stepper.get_past_mcu_position + self.mcu_to_commanded_position = self.stepper.mcu_to_commanded_position + self.dump_steps = self.stepper.dump_steps + self.get_stepper_kinematics = self.stepper.get_stepper_kinematics + self.set_stepper_kinematics = self.stepper.set_stepper_kinematics + self.note_homing_end = self.stepper.note_homing_end + self.calc_position_from_coord = self.stepper.calc_position_from_coord + self.generate_steps = self.stepper.generate_steps + self.add_active_callback = self.stepper.add_active_callback + self.is_active_axis = self.stepper.is_active_axis + def get_kin_coeffs(self): + return tuple(self.kin_coeffs) + def get_active_axes(self): + return [i for i, c in enumerate(self.kin_coeffs) if c] + def get_carriage(self): + return self.carriage + def add_to_rail(self, i, rail): + active_axes = self.get_active_axes() + endstop_pin = None + if rail.get_name() == self.get_carriage() or active_axes == [i]: + endstop_pin = self.config.get('endstop_pin', None) + rail.add_stepper(self, endstop_pin) + + +def LoadKinematicSteppers(config): + return [KinematicStepper(c) + for c in config.get_prefix_sections('kinematic_stepper')] diff --git a/klippy/kinematics/cartesian.py b/klippy/kinematics/cartesian.py index 0c4bb9255853..079a0c2745c2 100644 --- a/klippy/kinematics/cartesian.py +++ b/klippy/kinematics/cartesian.py @@ -11,7 +11,6 @@ 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'] @@ -24,19 +23,20 @@ def __init__(self, toolhead, config): if config.has_section('dual_carriage'): dc_config = config.getsection('dual_carriage') dc_axis = dc_config.getchoice('axis', ['x', 'y']) - self.dual_carriage_axis = {'x': 0, 'y': 1}[dc_axis] + dual_carriage_axis = {'x': 0, 'y': 1}[dc_axis] # setup second dual carriage rail self.rails.append(stepper.LookupMultiRail(dc_config)) self.rails[3].setup_itersolve('cartesian_stepper_alloc', dc_axis.encode()) dc_rail_0 = idex_modes.DualCarriagesRail( - self.rails[self.dual_carriage_axis], - axis=self.dual_carriage_axis, active=True) + self.rails[dual_carriage_axis], + self.rails[dual_carriage_axis].get_steppers(), + axis=dual_carriage_axis, active=True) dc_rail_1 = idex_modes.DualCarriagesRail( - self.rails[3], axis=self.dual_carriage_axis, active=False) + self.rails[3], self.rails[3].get_steppers(), + axis=dual_carriage_axis, active=False) self.dc_module = idex_modes.DualCarriages( - dc_config, dc_rail_0, dc_rail_1, - axis=self.dual_carriage_axis) + dc_config, dc_rail_0, dc_rail_1, axis=dual_carriage_axis) for s in self.get_steppers(): s.set_trapq(toolhead.get_trapq()) toolhead.register_step_generator(s.generate_steps) @@ -55,8 +55,8 @@ def calc_position(self, stepper_positions): rails = self.rails if self.dc_module: primary_rail = self.dc_module.get_primary_rail().get_rail() - rails = (rails[:self.dc_module.axis] + - [primary_rail] + rails[self.dc_module.axis+1:]) + rails = (rails[:self.dc_module.get_axis()] + + [primary_rail] + rails[self.dc_module.get_axis()+1:]) return [stepper_positions[rail.get_name()] for rail in rails] def update_limits(self, i, range): l, h = self.limits[i] @@ -68,7 +68,7 @@ def set_position(self, newpos, homing_axes): for i, rail in enumerate(self.rails): rail.set_position(newpos) for axis in homing_axes: - if self.dc_module and axis == self.dc_module.axis: + if self.dc_module and axis == self.dc_module.get_axis(): rail = self.dc_module.get_primary_rail().get_rail() else: rail = self.rails[axis] @@ -92,7 +92,7 @@ def home_axis(self, homing_state, axis, rail): def home(self, homing_state): # Each axis is homed independently and in order for axis in homing_state.get_axes(): - if self.dc_module is not None and axis == self.dual_carriage_axis: + if self.dc_module is not None and axis == self.dc_module.get_axis(): self.dc_module.home(homing_state) else: self.home_axis(homing_state, axis, self.rails[axis]) diff --git a/klippy/kinematics/generic_cartesian.py b/klippy/kinematics/generic_cartesian.py new file mode 100644 index 000000000000..266a743c2589 --- /dev/null +++ b/klippy/kinematics/generic_cartesian.py @@ -0,0 +1,205 @@ +# Code for generic handling the kinematics of cartesian-like printers +# +# Copyright (C) 2024 Dmitry Butyugin +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import mathutil, stepper +import gcode +import extras.kinematic_stepper as ks +from . import idex_modes + +def mat_mul(a, b): + if len(a[0]) != len(b): + return None + res = [] + for i in range(len(a)): + res.append([]) + for j in range(len(b[0])): + 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 + +class GenericCartesianKinematics: + def __init__(self, toolhead, config): + self.printer = config.get_printer() + self.rails = [stepper.GenericPrinterRail(config.getsection('rail_' + n)) + for n in 'xyz'] + self.dc_module = dual_carriage_axis = None + if config.has_section('dual_carriage'): + dc_config = config.getsection('dual_carriage') + dual_carriage_axis = dc_config.getchoice('axis', {'x': 0, 'y': 1}) + # setup second dual carriage rail + self.rails.append(stepper.GenericPrinterRail(dc_config)) + self.steppers = ks.LoadKinematicSteppers(config) + allowed_carriages = [r.get_name() for r in self.rails] + [None] + primary_carriage_steppers = [] + dual_carriage_steppers = [] + dc_rails = [None] + for s in self.steppers: + carriage = s.get_carriage() + if carriage not in allowed_carriages: + raise config.error( + "Invalid carriage '%s' for kinematic_stepper '%s'" % ( + carriage, s.get_name())) + for i in s.get_active_axes(): + if i == dual_carriage_axis: + dc_rails = [self.rails[i], self.rails[3]] + if dc_rails[0].get_name() == carriage: + s.add_to_rail(i, dc_rails[0]) + primary_carriage_steppers.append(s) + elif dc_rails[1].get_name() == carriage: + s.add_to_rail(i, dc_rails[1]) + dual_carriage_steppers.append(s) + else: + s.add_to_rail(i, dc_rails[0]) + s.add_to_rail(i, dc_rails[1]) + else: + s.add_to_rail(i, self.rails[i]) + s.set_trapq(toolhead.get_trapq()) + toolhead.register_step_generator(s.generate_steps) + if dual_carriage_axis is not None: + if not primary_carriage_steppers: + raise config.error( + "At least one kinematic_stepper must be declared with " + "'carriage: %s' when '[dual_carriage]' is used" % ( + self.rails[dual_carriage_axis].get_name())) + if not dual_carriage_steppers: + raise config.error( + "At least one kinematic_stepper must be declared with " + "'carriage: %s' when '[dual_carriage]' is used" % ( + self.rails[3].get_name())) + dc_rail_0 = idex_modes.DualCarriagesRail( + self.rails[dual_carriage_axis], primary_carriage_steppers, + axis=dual_carriage_axis, active=True) + dc_rail_1 = idex_modes.DualCarriagesRail( + self.rails[3], dual_carriage_steppers, + axis=dual_carriage_axis, active=False) + self.dc_module = idex_modes.DualCarriages( + dc_config, dc_rail_0, dc_rail_1, axis=dual_carriage_axis) + if not all(self._check_kinematics(skip_dc_rail=r) for r in dc_rails): + raise config.error( + "Verify configured 'kinematic_stepper'(s) and their " + "'kinematics' specifications, the current configuration " + "does not allow independent movements of all printer axes.") + for r in self.rails: + r.add_steppers_to_endstops() + 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 self.steppers + def get_primary_rails(self): + rails = self.rails + if self.dc_module: + primary_rail = self.dc_module.get_primary_rail().get_rail() + return (rails[:self.dc_module.get_axis()] + [primary_rail] + + rails[self.dc_module.get_axis()+1:3]) + return rails + def get_active_steppers(self): + return {s.get_name() : s for s in self.steppers + if self.dc_module is None or s.get_carriage() is None + or all(self.dc_module.get_status()['carriage_%d' % i] != + 'INACTIVE' for i in [0, 1] if s.get_carriage() == + self.dc_module.get_rails()[i].get_rail().get_name())} + def _check_kinematics(self, skip_dc_rail): + matr = [s.get_kin_coeffs() for s in self.steppers + if skip_dc_rail is None or s.get_carriage() is None + or s.get_carriage() != skip_dc_rail.get_name()] + det = mathutil.matrix_det(mat_mul(mat_transp(matr), matr)) + return abs(det) > 0.00001 + def calc_position(self, stepper_positions): + steppers = self.get_active_steppers() + matr = [s.get_kin_coeffs() for s in steppers.values()] + inv = mathutil.matrix_inv(mat_mul(mat_transp(matr), matr)) + proj = mat_mul(matr, inv) + spos = [stepper_positions[sname] for sname in steppers.keys()] + pos = mat_mul([spos], proj) + 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.steppers: + s.set_position(newpos) + for axis in homing_axes: + if self.dc_module and axis == self.dc_module.get_axis(): + rail = self.dc_module.get_primary_rail().get_rail() + else: + rail = self.rails[axis] + self.limits[axis] = 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 self.dc_module is not None and axis == self.dc_module.get_axis(): + self.dc_module.home(homing_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] + ranges = [r.get_range() for r in self.get_primary_rails()] + axes_min = gcode.Coord(*[r[0] for r in ranges], e=0.) + axes_max = gcode.Coord(*[r[1] for r in ranges], e=0.) + return { + 'homed_axes': "".join(axes), + 'axis_minimum': axes_min, + 'axis_maximum': axes_max, + } + +def load_kinematics(toolhead, config): + return GenericCartesianKinematics(toolhead, config) diff --git a/klippy/kinematics/hybrid_corexy.py b/klippy/kinematics/hybrid_corexy.py index 265a0e6da33d..0f14ecb7825a 100644 --- a/klippy/kinematics/hybrid_corexy.py +++ b/klippy/kinematics/hybrid_corexy.py @@ -34,9 +34,11 @@ def __init__(self, toolhead, config): self.rails[3].get_steppers()[0]) self.rails[3].setup_itersolve('corexy_stepper_alloc', b'+') dc_rail_0 = idex_modes.DualCarriagesRail( - self.rails[0], axis=0, active=True) + self.rails[0], self.rails[0].get_steppers(), + axis=0, active=True) dc_rail_1 = idex_modes.DualCarriagesRail( - self.rails[3], axis=0, active=False) + self.rails[3], self.rails[3].get_steppers(), + axis=0, active=False) self.dc_module = idex_modes.DualCarriages( dc_config, dc_rail_0, dc_rail_1, axis=0) for s in self.get_steppers(): diff --git a/klippy/kinematics/hybrid_corexz.py b/klippy/kinematics/hybrid_corexz.py index 2d89e3f7bd03..aac1e3dc6384 100644 --- a/klippy/kinematics/hybrid_corexz.py +++ b/klippy/kinematics/hybrid_corexz.py @@ -34,9 +34,11 @@ def __init__(self, toolhead, config): self.rails[3].get_steppers()[0]) self.rails[3].setup_itersolve('corexz_stepper_alloc', b'+') dc_rail_0 = idex_modes.DualCarriagesRail( - self.rails[0], axis=0, active=True) + self.rails[0], self.rails[0].get_steppers(), + axis=0, active=True) dc_rail_1 = idex_modes.DualCarriagesRail( - self.rails[3], axis=0, active=False) + self.rails[3], self.rails[3].get_steppers(), + axis=0, active=False) self.dc_module = idex_modes.DualCarriages( dc_config, dc_rail_0, dc_rail_1, axis=0) for s in self.get_steppers(): diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py index f2618d0805a7..4059aa8295e3 100644 --- a/klippy/kinematics/idex_modes.py +++ b/klippy/kinematics/idex_modes.py @@ -40,6 +40,8 @@ def __init__(self, dc_config, rail_0, rail_1, axis): 'RESTORE_DUAL_CARRIAGE_STATE', self.cmd_RESTORE_DUAL_CARRIAGE_STATE, desc=self.cmd_RESTORE_DUAL_CARRIAGE_STATE_help) + def get_axis(self): + return self.axis def get_rails(self): return self.dc def get_primary_rail(self): @@ -216,7 +218,7 @@ def cmd_RESTORE_DUAL_CARRIAGE_STATE(self, gcmd): class DualCarriagesRail: ENC_AXES = [b'x', b'y'] - def __init__(self, rail, axis, active): + def __init__(self, rail, steppers, axis, active): self.rail = rail self.axis = axis self.mode = (INACTIVE, PRIMARY)[active] @@ -225,7 +227,7 @@ def __init__(self, rail, axis, active): ffi_main, ffi_lib = chelper.get_ffi() self.dc_stepper_kinematics = [] self.orig_stepper_kinematics = [] - for s in rail.get_steppers(): + for s in steppers: sk = ffi_main.gc(ffi_lib.dual_carriage_alloc(), ffi_lib.free) orig_sk = s.get_stepper_kinematics() ffi_lib.dual_carriage_set_sk(sk, orig_sk) diff --git a/klippy/mathutil.py b/klippy/mathutil.py index a6ab50d224ed..c741d915408e 100644 --- a/klippy/mathutil.py +++ b/klippy/mathutil.py @@ -135,3 +135,18 @@ def matrix_sub(m1, m2): def matrix_mul(m1, s): return [m1[0]*s, m1[1]*s, m1[2]*s] + +###################################################################### +# Matrix helper functions for 3x3 matrices +###################################################################### + +def matrix_det(a): + x0, x1, x2 = a + return matrix_dot(x0, matrix_cross(x1, x2)) + +def matrix_inv(a): + x0, x1, x2 = a + inv_det = 1. / matrix_det(a) + return [matrix_mul(matrix_cross(x1, x2), inv_det), + matrix_mul(matrix_cross(x2, x0), inv_det), + matrix_mul(matrix_cross(x0, x1), inv_det)] diff --git a/klippy/stepper.py b/klippy/stepper.py index 9b692904dcf9..9bd71c1b75ef 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -294,28 +294,18 @@ def parse_step_distance(config, units_in_radians=None, note_valid=False): # A motor control "rail" with one (or more) steppers and one (or more) # endstops. -class PrinterRail: +class GenericPrinterRail: def __init__(self, config, need_position_minmax=True, - default_position_endstop=None, units_in_radians=False): - # Primary stepper and endstop - self.stepper_units_in_radians = units_in_radians + default_position_endstop=None): + self.config = config + self.name = config.get_name() self.steppers = [] self.endstops = [] + self.steppers_pending_endstops = [] self.endstop_map = {} - self.add_extra_stepper(config) - mcu_stepper = self.steppers[0] - self.get_name = mcu_stepper.get_name - self.get_commanded_position = mcu_stepper.get_commanded_position - self.calc_position_from_coord = mcu_stepper.calc_position_from_coord - # Primary endstop position - mcu_endstop = self.endstops[0][0] - if hasattr(mcu_endstop, "get_position_endstop"): - self.position_endstop = mcu_endstop.get_position_endstop() - elif default_position_endstop is None: - self.position_endstop = config.getfloat('position_endstop') - else: - self.position_endstop = config.getfloat( - 'position_endstop', default_position_endstop) + self.position_endstop = config.getfloat( + 'position_endstop', default_position_endstop) + self.rail_endstop_pin = config.get('endstop_pin', None) # Axis range if need_position_minmax: self.position_min = config.getfloat('position_min', 0.) @@ -324,11 +314,6 @@ def __init__(self, config, need_position_minmax=True, else: self.position_min = 0. self.position_max = self.position_endstop - if (self.position_endstop < self.position_min - or self.position_endstop > self.position_max): - raise config.error( - "position_endstop in section '%s' must be between" - " position_min and position_max" % config.get_name()) # Homing mechanics self.homing_speed = config.getfloat('homing_speed', 5.0, above=0.) self.second_homing_speed = config.getfloat( @@ -339,6 +324,26 @@ def __init__(self, config, need_position_minmax=True, 'homing_retract_dist', 5., minval=0.) self.homing_positive_dir = config.getboolean( 'homing_positive_dir', None) + def get_name(self): + return self.name + def _determine_position_endstop(self): + # Primary endstop position + mcu_endstop = self.endstops[0][0] + if hasattr(mcu_endstop, "get_position_endstop"): + if self.position_endstop is not None: + raise self.config.error( + "position_endstop is specified in section " + "'%s', but it should not be" % self.name) + self.position_endstop = mcu_endstop.get_position_endstop() + elif self.position_endstop is None: + raise self.config.error( + "position_endstop must be specified in section " + "'%s'" % self.name) + if (self.position_endstop < self.position_min + or self.position_endstop > self.position_max): + raise self.config.error( + "position_endstop in section '%s' must be between" + " position_min and position_max" % self.name) if self.homing_positive_dir is None: axis_len = self.position_max - self.position_min if self.position_endstop <= self.position_min + axis_len / 4.: @@ -346,17 +351,16 @@ def __init__(self, config, need_position_minmax=True, elif self.position_endstop >= self.position_max - axis_len / 4.: self.homing_positive_dir = True else: - raise config.error( + raise config_error( "Unable to infer homing_positive_dir in section '%s'" - % (config.get_name(),)) - config.getboolean('homing_positive_dir', self.homing_positive_dir) + % (self.name,)) elif ((self.homing_positive_dir and self.position_endstop == self.position_min) or (not self.homing_positive_dir and self.position_endstop == self.position_max)): - raise config.error( + raise self.config.error( "Invalid homing_positive_dir / position_endstop in '%s'" - % (config.get_name(),)) + % (self.name,)) def get_range(self): return self.position_min, self.position_max def get_homing_info(self): @@ -371,15 +375,8 @@ def get_steppers(self): return list(self.steppers) def get_endstops(self): return list(self.endstops) - def add_extra_stepper(self, config): - stepper = PrinterStepper(config, self.stepper_units_in_radians) - self.steppers.append(stepper) - if self.endstops and config.get('endstop_pin', None) is None: - # No endstop defined - use primary endstop - self.endstops[0][0].add_stepper(stepper) - return - endstop_pin = config.get('endstop_pin') - printer = config.get_printer() + def lookup_endstop(self, endstop_pin, name): + printer = self.config.get_printer() ppins = printer.lookup_object('pins') pin_params = ppins.parse_pin(endstop_pin, True, True) # Normalize pin name @@ -392,19 +389,56 @@ def add_extra_stepper(self, config): self.endstop_map[pin_name] = {'endstop': mcu_endstop, 'invert': pin_params['invert'], 'pullup': pin_params['pullup']} - name = stepper.get_name(short=True) self.endstops.append((mcu_endstop, name)) - query_endstops = printer.load_object(config, 'query_endstops') + query_endstops = printer.load_object(self.config, 'query_endstops') query_endstops.register_endstop(mcu_endstop, name) else: mcu_endstop = endstop['endstop'] changed_invert = pin_params['invert'] != endstop['invert'] changed_pullup = pin_params['pullup'] != endstop['pullup'] if changed_invert or changed_pullup: - raise error("Pinter rail %s shared endstop pin %s " - "must specify the same pullup/invert settings" % ( - self.get_name(), pin_name)) + raise self.config.error( + "Printer rail %s shared endstop pin %s " + "must specify the same pullup/invert settings" % ( + self.get_name(), pin_name)) + return mcu_endstop + def add_stepper(self, stepper, endstop_pin=None): + self.steppers.append(stepper) + first_stepper_with_endstop = not self.endstops + if endstop_pin is not None: + mcu_endstop = self.lookup_endstop(endstop_pin, + stepper.get_name(short=True)) + elif self.rail_endstop_pin is not None: + mcu_endstop = self.lookup_endstop(self.rail_endstop_pin, self.name) + else: + self.steppers_pending_endstops.append(stepper) + return mcu_endstop.add_stepper(stepper) + # Primary endstop position + if first_stepper_with_endstop: + self._determine_position_endstop() + def add_steppers_to_endstops(self): + if not self.endstops: + raise self.config.error( + "endstop_pin must be specfied in section '%s'" % self.name) + for s in self.steppers_pending_endstops: + self.endstops[0][0].add_stepper(s) + del self.steppers_pending_endstops[:] + +class PrinterRail(GenericPrinterRail): + def __init__(self, config, need_position_minmax=True, + default_position_endstop=None, units_in_radians=False): + GenericPrinterRail.__init__(self, config, need_position_minmax, + default_position_endstop) + self.stepper_units_in_radians = units_in_radians + self.add_extra_stepper(config) + mcu_stepper = self.steppers[0] + self.get_name = mcu_stepper.get_name + self.get_commanded_position = mcu_stepper.get_commanded_position + self.calc_position_from_coord = mcu_stepper.calc_position_from_coord + def add_extra_stepper(self, config): + stepper = PrinterStepper(config, self.stepper_units_in_radians) + self.add_stepper(stepper, config.get('endstop_pin', None)) def setup_itersolve(self, alloc_func, *params): for stepper in self.steppers: stepper.setup_itersolve(alloc_func, *params) @@ -420,7 +454,7 @@ def set_position(self, coord): # Wrapper for dual stepper motor support def LookupMultiRail(config, need_position_minmax=True, - default_position_endstop=None, units_in_radians=False): + default_position_endstop=None, units_in_radians=False): rail = PrinterRail(config, need_position_minmax, default_position_endstop, units_in_radians) for i in range(1, 99):