diff options
Diffstat (limited to 'klippy/kinematics')
| -rw-r--r-- | klippy/kinematics/cartesian.py | 83 | ||||
| -rw-r--r-- | klippy/kinematics/hybrid_corexy.py | 30 | ||||
| -rw-r--r-- | klippy/kinematics/hybrid_corexz.py | 30 | ||||
| -rw-r--r-- | klippy/kinematics/idex_modes.py | 273 |
4 files changed, 237 insertions, 179 deletions
diff --git a/klippy/kinematics/cartesian.py b/klippy/kinematics/cartesian.py index 0cd7849a..59604197 100644 --- a/klippy/kinematics/cartesian.py +++ b/klippy/kinematics/cartesian.py @@ -5,6 +5,7 @@ # This file may be distributed under the terms of the GNU GPLv3 license. import logging import stepper +from . import idex_modes class CartKinematics: def __init__(self, toolhead, config): @@ -16,6 +17,25 @@ class CartKinematics: for n in 'xyz'] for rail, axis in zip(self.rails, 'xyz'): rail.setup_itersolve('cartesian_stepper_alloc', axis.encode()) + 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') + dc_axis = dc_config.getchoice('axis', {'x': 'x', 'y': 'y'}) + self.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[0], axis=self.dual_carriage_axis, active=True) + dc_rail_1 = idex_modes.DualCarriagesRail( + self.rails[3], axis=self.dual_carriage_axis, active=False) + self.dc_module = idex_modes.DualCarriages( + dc_config, dc_rail_0, dc_rail_1, + axis=self.dual_carriage_axis) for s in self.get_steppers(): s.set_trapq(toolhead.get_trapq()) toolhead.register_step_generator(s.generate_steps) @@ -28,31 +48,18 @@ class CartKinematics: 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()] + return [s for rail in self.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 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 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) @@ -61,7 +68,7 @@ class CartKinematics: 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): + def home_axis(self, homing_state, axis, rail): # Determine movement position_min, position_max = rail.get_range() hi = rail.get_homing_info() @@ -77,16 +84,10 @@ class CartKinematics: 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) + if self.dc_module is not None and axis == self.dual_carriage_axis: + self.dc_module.home(homing_state) else: - self._home_axis(homing_state, axis, self.rails[axis]) + 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): @@ -119,24 +120,6 @@ class CartKinematics: '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) diff --git a/klippy/kinematics/hybrid_corexy.py b/klippy/kinematics/hybrid_corexy.py index 62dccc26..26032039 100644 --- a/klippy/kinematics/hybrid_corexy.py +++ b/klippy/kinematics/hybrid_corexy.py @@ -33,17 +33,13 @@ class HybridCoreXYKinematics: 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') + self.rails[3].setup_itersolve('corexy_stepper_alloc', b'+') 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')) + self.rails[0], axis=0, active=True) 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) + self.rails[3], 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(): s.set_trapq(toolhead.get_trapq()) toolhead.register_step_generator(s.generate_steps) @@ -60,8 +56,8 @@ class HybridCoreXYKinematics: 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']): + if (self.dc_module is not None and 'PRIMARY' == \ + self.dc_module.get_status()['carriage_1']): return [pos[0] - pos[1], pos[1], pos[2]] else: return [pos[0] + pos[1], pos[1], pos[2]] @@ -81,7 +77,7 @@ class HybridCoreXYKinematics: 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): + 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] @@ -95,14 +91,10 @@ class HybridCoreXYKinematics: 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() + if self.dc_module is not None and axis == 0: + self.dc_module.home(homing_state) else: - self._home_axis(homing_state, axis, self.rails[axis]) + 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): diff --git a/klippy/kinematics/hybrid_corexz.py b/klippy/kinematics/hybrid_corexz.py index a92a2194..e13c9aa4 100644 --- a/klippy/kinematics/hybrid_corexz.py +++ b/klippy/kinematics/hybrid_corexz.py @@ -33,17 +33,13 @@ class HybridCoreXZKinematics: 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') + self.rails[3].setup_itersolve('corexz_stepper_alloc', b'+') 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')) + self.rails[0], axis=0, active=True) 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) + self.rails[3], 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(): s.set_trapq(toolhead.get_trapq()) toolhead.register_step_generator(s.generate_steps) @@ -60,8 +56,8 @@ class HybridCoreXZKinematics: 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']): + if (self.dc_module is not None and 'PRIMARY' == \ + self.dc_module.get_status()['carriage_1']): return [pos[0] - pos[2], pos[1], pos[2]] else: return [pos[0] + pos[2], pos[1], pos[2]] @@ -81,7 +77,7 @@ class HybridCoreXZKinematics: 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): + 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] @@ -95,14 +91,10 @@ class HybridCoreXZKinematics: 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() + if self.dc_module is not None and axis == 0: + self.dc_module.home(homing_state) else: - self._home_axis(homing_state, axis, self.rails[axis]) + 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): 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 |