diff options
Diffstat (limited to 'klippy/kinematics/kinematic_stepper.py')
| -rw-r--r-- | klippy/kinematics/kinematic_stepper.py | 92 |
1 files changed, 92 insertions, 0 deletions
diff --git a/klippy/kinematics/kinematic_stepper.py b/klippy/kinematics/kinematic_stepper.py new file mode 100644 index 00000000..c82f0855 --- /dev/null +++ b/klippy/kinematics/kinematic_stepper.py @@ -0,0 +1,92 @@ +# Kinematic stepper class for generic cartesian kinematics +# +# Copyright (C) 2024 Dmitry Butyugin <dmbutyugin@google.com> +# +# This file may be distributed under the terms of the GNU GPLv3 license. + +import logging, re +import stepper, chelper + +def parse_carriages_string(carriages_str, printer_carriages, parse_error): + nxt = 0 + pat = re.compile('[+-]') + coeffs = [0.] * 3 + ref_carriages = [] + while nxt < len(carriages_str): + match = pat.search(carriages_str, nxt+1) + end = len(carriages_str) if match is None else match.start() + term = carriages_str[nxt:end].strip() + term_lst = term.split('*') + if len(term_lst) not in [1, 2]: + raise parse_error( + "Invalid term '%s' in '%s'" % (term, carriages_str)) + 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. + if term_lst[0].startswith('-') or term_lst[0].startswith('+'): + term_lst[0] = term_lst[0][1:] + c = term_lst[-1] + if c not in printer_carriages: + raise parse_error("Invalid '%s' carriage referenced in '%s'" % + (c, carriages_str)) + carriage = printer_carriages[c] + j = carriage.get_axis() + if coeffs[j]: + raise error("Carriage '%s' was referenced multiple times in '%s'" % + (c, carriages_str)) + coeffs[j] = coeff + ref_carriages.append(carriage) + nxt = end + return coeffs, ref_carriages + +class KinematicStepper: + def __init__(self, config, printer_carriages): + self.printer = config.get_printer() + self.stepper = stepper.PrinterStepper(config) + self.kin_coeffs, self.carriages = parse_carriages_string( + config.get('carriages'), printer_carriages, config.error) + if not any(self.kin_coeffs): + raise config.error( + "'%s' must provide a valid 'carriages' configuration" % + self.stepper.get_name()) + self.stepper.setup_itersolve( + 'generic_cartesian_stepper_alloc', + self.kin_coeffs[0], self.kin_coeffs[1], self.kin_coeffs[2]) + self.stepper_sk = self.stepper.get_stepper_kinematics() + # Add stepper to the carriages it references + for sc in self.carriages: + sc.add_stepper(self) + def get_name(self, short=False): + name = self.stepper.get_name(short) + if short and name.startswith('stepper '): + return name[8:] + return name + def get_stepper(self): + return self.stepper + 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_carriages(self): + return self.carriages + def update_kin_coeffs(self, kin_coeffs): + self.kin_coeffs = kin_coeffs + ffi_main, ffi_lib = chelper.get_ffi() + ffi_lib.generic_cartesian_stepper_set_coeffs( + self.stepper_sk, kin_coeffs[0], kin_coeffs[1], kin_coeffs[2]) + def update_carriages(self, carriages_str, printer_carriages, report_error): + kin_coeffs, carriages = parse_carriages_string( + carriages_str, printer_carriages, + report_error or self.printer.command_error) + if report_error is not None and not any(kin_coeffs): + raise report_error( + "A valid string that references at least one carriage" + " must be provided for '%s'" % self.get_name()) + self.carriages = carriages + self.update_kin_coeffs(kin_coeffs) + def set_position(self, coord): + self.stepper.set_position(coord) |