diff options
Diffstat (limited to 'klippy/kinematics/idex_modes.py')
| -rw-r--r-- | klippy/kinematics/idex_modes.py | 273 |
1 files changed, 182 insertions, 91 deletions
diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py index 73fac776..ec8944bb 100644 --- a/klippy/kinematics/idex_modes.py +++ b/klippy/kinematics/idex_modes.py @@ -1,23 +1,48 @@ # Support for duplication and mirroring modes for IDEX printers # # Copyright (C) 2021 Fabrice Gallet <tircown@gmail.com> +# Copyright (C) 2023 Dmitry Butyugin <dmbutyugin@google.com> # # This file may be distributed under the terms of the GNU GPLv3 license. import math import chelper +INACTIVE = 'INACTIVE' +PRIMARY = 'PRIMARY' +COPY = 'COPY' +MIRROR = 'MIRROR' + class DualCarriages: - def __init__(self, printer, rail_0, rail_1, axis): - self.printer = printer + VALID_MODES = [PRIMARY, COPY, MIRROR] + def __init__(self, dc_config, rail_0, rail_1, axis): + self.printer = dc_config.get_printer() self.axis = axis self.dc = (rail_0, rail_1) - self.saved_state = None + self.saved_states = {} + safe_dist = dc_config.getfloat('safe_distance', None, minval=0.) + if safe_dist is None: + dc0_rail = rail_0.get_rail() + dc1_rail = rail_1.get_rail() + safe_dist = min(abs(dc0_rail.position_min - dc1_rail.position_min), + abs(dc0_rail.position_max - dc1_rail.position_max)) + self.safe_dist = safe_dist self.printer.add_object('dual_carriage', self) + self.printer.register_event_handler("klippy:ready", self._handle_ready) 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): + gcode.register_command( + 'SAVE_DUAL_CARRIAGE_STATE', + self.cmd_SAVE_DUAL_CARRIAGE_STATE, + desc=self.cmd_SAVE_DUAL_CARRIAGE_STATE_help) + gcode.register_command( + 'RESTORE_DUAL_CARRIAGE_STATE', + self.cmd_RESTORE_DUAL_CARRIAGE_STATE, + desc=self.cmd_RESTORE_DUAL_CARRIAGE_STATE_help) + def get_rails(self): + return self.dc + def toggle_active_dc_rail(self, index, override_rail=False): toolhead = self.printer.lookup_object('toolhead') toolhead.flush_step_generation() pos = toolhead.get_position() @@ -27,104 +52,170 @@ class DualCarriages: if i != index: if dc.is_active(): 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()) + if override_rail: + kin.override_rail(3, dc_rail) + target_dc = self.dc[index] + if target_dc.mode != PRIMARY: + newpos = pos[:self.axis] + [target_dc.get_axis_position(pos)] \ + + pos[self.axis+1:] + target_dc.activate(PRIMARY, newpos, old_position=pos) + if override_rail: + kin.override_rail(self.axis, target_dc.get_rail()) + toolhead.set_position(newpos) + kin.update_limits(self.axis, target_dc.get_rail().get_range()) + def home(self, homing_state): + kin = self.printer.lookup_object('toolhead').get_kinematics() + for i, dc_rail in enumerate(self.dc): + self.toggle_active_dc_rail(i, override_rail=True) + kin.home_axis(homing_state, self.axis, dc_rail.get_rail()) + # Restore the original rails ordering + self.toggle_active_dc_rail(0, override_rail=True) def get_status(self, eventtime=None): - dc0, dc1 = self.dc - if (dc0.is_active() is True): - return { 'mode': 'FULL_CONTROL', 'active_carriage': 'CARRIAGE_0' } + return {('carriage_%d' % (i,)) : dc.mode + for (i, dc) in enumerate(self.dc)} + def get_kin_range(self, toolhead, mode): + pos = toolhead.get_position() + axes_pos = [dc.get_axis_position(pos) for dc in self.dc] + dc0_rail = self.dc[0].get_rail() + dc1_rail = self.dc[1].get_rail() + range_min = dc0_rail.position_min + range_max = dc0_rail.position_max + safe_dist = self.safe_dist + + if mode == COPY: + range_min = max(range_min, + axes_pos[0] - axes_pos[1] + dc1_rail.position_min) + range_max = min(range_max, + axes_pos[0] - axes_pos[1] + dc1_rail.position_max) + elif mode == MIRROR: + if dc0_rail.get_homing_info().positive_dir: + range_min = max(range_min, + 0.5 * (sum(axes_pos) + safe_dist)) + range_max = min(range_max, + sum(axes_pos) - dc1_rail.position_min) + else: + range_max = min(range_max, + 0.5 * (sum(axes_pos) - safe_dist)) + range_min = max(range_min, + sum(axes_pos) - dc1_rail.position_max) 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') + # mode == PRIMARY + active_idx = 1 if self.dc[1].is_active() else 0 + inactive_idx = 1 - active_idx + if active_idx: + range_min = dc1_rail.position_min + range_max = dc1_rail.position_max + if self.dc[active_idx].get_rail().get_homing_info().positive_dir: + range_min = max(range_min, axes_pos[inactive_idx] + safe_dist) + else: + range_max = min(range_max, axes_pos[inactive_idx] - safe_dist) + return (range_min, range_max) + def activate_dc_mode(self, index, mode): + toolhead = self.printer.lookup_object('toolhead') + toolhead.flush_step_generation() + kin = toolhead.get_kinematics() + if mode == INACTIVE: + self.dc[index].inactivate(toolhead.get_position()) + elif mode == PRIMARY: + self.toggle_active_dc_rail(index) 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" + self.toggle_active_dc_rail(0) + self.dc[index].activate(mode, toolhead.get_position()) + kin.update_limits(self.axis, self.get_kin_range(toolhead, mode)) + def _handle_ready(self): + # Apply the transform later during Klipper initialization to make sure + # that input shaping can pick up the correct stepper kinematic flags. + for dc in self.dc: + dc.apply_transform() + cmd_SET_DUAL_CARRIAGE_help = "Configure the dual carriages mode" 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) + mode = gcmd.get('MODE', PRIMARY).upper() + if mode not in self.VALID_MODES: + raise gcmd.error("Invalid mode=%s specified" % (mode,)) + if mode in [COPY, MIRROR]: + if index == 0: + raise gcmd.error( + "Mode=%s is not supported for carriage=0" % (mode,)) + curtime = self.printer.get_reactor().monotonic() + kin = self.printer.lookup_object('toolhead').get_kinematics() + axis = 'xyz'[self.axis] + if axis not in kin.get_status(curtime)['homed_axes']: + raise gcmd.error( + "Axis %s must be homed prior to enabling mode=%s" % + (axis, mode)) + self.activate_dc_mode(index, mode) + cmd_SAVE_DUAL_CARRIAGE_STATE_help = \ + "Save dual carriages modes and positions" + def cmd_SAVE_DUAL_CARRIAGE_STATE(self, gcmd): + state_name = gcmd.get('NAME', 'default') + pos = self.printer.lookup_object('toolhead').get_position() + self.saved_states[state_name] = { + 'carriage_modes': [dc.mode for dc in self.dc], + 'axes_positions': [dc.get_axis_position(pos) for dc in self.dc], + } + cmd_RESTORE_DUAL_CARRIAGE_STATE_help = \ + "Restore dual carriages modes and positions" + def cmd_RESTORE_DUAL_CARRIAGE_STATE(self, gcmd): + state_name = gcmd.get('NAME', 'default') + saved_state = self.saved_states.get(state_name) + if saved_state is None: + raise gcmd.error("Unknown DUAL_CARRIAGE state: %s" % (state_name,)) + move_speed = gcmd.get('MOVE_SPEED', 0., above=0.) + toolhead = self.printer.lookup_object('toolhead') + toolhead.flush_step_generation() + pos = toolhead.get_position() + if gcmd.get_int('MOVE', 1): + for i, dc in enumerate(self.dc): + self.toggle_active_dc_rail(i) + saved_pos = saved_state['axes_positions'][i] + toolhead.manual_move( + pos[:self.axis] + [saved_pos] + pos[self.axis+1:], + move_speed or dc.get_rail().homing_speed) + for i, dc in enumerate(self.dc): + saved_mode = saved_state['carriage_modes'][i] + self.activate_dc_mode(i, saved_mode) class DualCarriagesRail: - ACTIVE=1 - INACTIVE=2 - def __init__(self, printer, rail, axis, active, stepper_alloc_active, - stepper_alloc_inactive=None): - self.printer = printer + ENC_AXES = [b'x', b'y'] + def __init__(self, rail, axis, active): 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 - self.stepper_active_sk = {} - self.stepper_inactive_sk = {} - for s in rail.get_steppers(): - self._save_sk(self.status, s, s.get_stepper_kinematics()) - def _alloc_sk(self, alloc_func, *params): + self.mode = (INACTIVE, PRIMARY)[active] + self.offset = 0. + self.scale = 1. if active else 0. ffi_main, ffi_lib = chelper.get_ffi() - return ffi_main.gc(getattr(ffi_lib, alloc_func)(*params), ffi_lib.free) - def _get_sk(self, status, stepper): - sk = None - if status == self.ACTIVE: - sk = self.stepper_active_sk.get(stepper, None) - if sk is None and self.stepper_alloc_active: - sk = self._alloc_sk(*self.stepper_alloc_active) - self._save_sk(status, stepper, sk) - elif status == self.INACTIVE: - sk = self.stepper_inactive_sk.get(stepper, None) - if sk is None and self.stepper_alloc_inactive: - sk = self._alloc_sk(*self.stepper_alloc_inactive) - self._save_sk(status, stepper, sk) - return sk - def _save_sk(self, status, stepper, sk): - if status == self.ACTIVE: - self.stepper_active_sk[stepper] = sk - elif status == self.INACTIVE: - self.stepper_inactive_sk[stepper] = sk - def _update_stepper_alloc(self, position, active=True): - toolhead = self.printer.lookup_object('toolhead') - self.axis_position = position[self.axis] - self.rail.set_trapq(None) - old_status = self.status - self.status = (self.INACTIVE, self.ACTIVE)[active] - for s in self.rail.get_steppers(): - sk = self._get_sk(self.status, s) - if sk is None: - return - old_sk = s.set_stepper_kinematics(sk) - self._save_sk(old_status, s, old_sk) - self.rail.set_position(position) - self.rail.set_trapq(toolhead.get_trapq()) + self.dc_stepper_kinematics = [] + self.orig_stepper_kinematics = [] + for s in rail.get_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) + # Set the default transform for the other axis + ffi_lib.dual_carriage_set_transform( + sk, self.ENC_AXES[1 - axis], 1., 0.) + self.dc_stepper_kinematics.append(sk) + self.orig_stepper_kinematics.append(orig_sk) + s.set_stepper_kinematics(sk) def get_rail(self): return self.rail def is_active(self): - return self.status == self.ACTIVE - def activate(self, position): - self._update_stepper_alloc(position, active=True) + return self.mode != INACTIVE + def get_axis_position(self, position): + return position[self.axis] * self.scale + self.offset + def apply_transform(self): + ffi_main, ffi_lib = chelper.get_ffi() + for sk in self.dc_stepper_kinematics: + ffi_lib.dual_carriage_set_transform( + sk, self.ENC_AXES[self.axis], self.scale, self.offset) + def activate(self, mode, position, old_position=None): + old_axis_position = self.get_axis_position(old_position or position) + self.scale = -1. if mode == MIRROR else 1. + self.offset = old_axis_position - position[self.axis] * self.scale + self.apply_transform() + self.mode = mode def inactivate(self, position): - self._update_stepper_alloc(position, active=False) + self.offset = self.get_axis_position(position) + self.scale = 0. + self.apply_transform() + self.mode = INACTIVE |