diff options
Diffstat (limited to 'klippy/kinematics/idex_modes.py')
| -rw-r--r-- | klippy/kinematics/idex_modes.py | 248 |
1 files changed, 149 insertions, 99 deletions
diff --git a/klippy/kinematics/idex_modes.py b/klippy/kinematics/idex_modes.py index 19d5a655..5910d5c0 100644 --- a/klippy/kinematics/idex_modes.py +++ b/klippy/kinematics/idex_modes.py @@ -7,29 +7,33 @@ import collections, logging, math import chelper -INACTIVE = 'INACTIVE' -PRIMARY = 'PRIMARY' -COPY = 'COPY' -MIRROR = 'MIRROR' +INACTIVE = "INACTIVE" +PRIMARY = "PRIMARY" +COPY = "COPY" +MIRROR = "MIRROR" + class DualCarriages: VALID_MODES = [PRIMARY, COPY, MIRROR] - def __init__(self, printer, primary_rails, dual_rails, axes, - safe_dist={}): + + def __init__(self, printer, primary_rails, dual_rails, axes, safe_dist={}): self.printer = printer self.axes = axes self._init_steppers(primary_rails + dual_rails) self.primary_rails = [ - DualCarriagesRail(printer, c, dual_rails[i], - axes[i], active=True) - for i, c in enumerate(primary_rails)] + DualCarriagesRail(printer, c, dual_rails[i], axes[i], active=True) + for i, c in enumerate(primary_rails) + ] self.dual_rails = [ - DualCarriagesRail(printer, c, primary_rails[i], - axes[i], active=False) - for i, c in enumerate(dual_rails)] + DualCarriagesRail(printer, c, primary_rails[i], axes[i], active=False) + for i, c in enumerate(dual_rails) + ] self.dc_rails = collections.OrderedDict( - [(c.rail.get_name(short=True), c) - for c in self.primary_rails + self.dual_rails]) + [ + (c.rail.get_name(short=True), c) + for c in self.primary_rails + self.dual_rails + ] + ) self.saved_states = {} self.safe_dist = {} for i, dc in enumerate(dual_rails): @@ -42,22 +46,29 @@ class DualCarriages: self.safe_dist[axis] = safe_dist continue pc = primary_rails[i] - self.safe_dist[axis] = min(abs(pc.position_min - dc.position_min), - abs(pc.position_max - dc.position_max)) - self.printer.add_object('dual_carriage', self) + self.safe_dist[axis] = min( + abs(pc.position_min - dc.position_min), + abs(pc.position_max - dc.position_max), + ) + self.printer.add_object("dual_carriage", self) self.printer.register_event_handler("klippy:ready", self._handle_ready) - gcode = self.printer.lookup_object('gcode') + gcode = self.printer.lookup_object("gcode") gcode.register_command( - 'SET_DUAL_CARRIAGE', self.cmd_SET_DUAL_CARRIAGE, - desc=self.cmd_SET_DUAL_CARRIAGE_help) + "SET_DUAL_CARRIAGE", + self.cmd_SET_DUAL_CARRIAGE, + desc=self.cmd_SET_DUAL_CARRIAGE_help, + ) gcode.register_command( - 'SAVE_DUAL_CARRIAGE_STATE', - self.cmd_SAVE_DUAL_CARRIAGE_STATE, - desc=self.cmd_SAVE_DUAL_CARRIAGE_STATE_help) + "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) + "RESTORE_DUAL_CARRIAGE_STATE", + self.cmd_RESTORE_DUAL_CARRIAGE_STATE, + desc=self.cmd_RESTORE_DUAL_CARRIAGE_STATE_help, + ) + def _init_steppers(self, rails): ffi_main, ffi_lib = chelper.get_ffi() self.dc_stepper_kinematics = [] @@ -67,8 +78,9 @@ class DualCarriages: c_steppers = rail.get_steppers() if not c_steppers: raise self.printer.config_error( - "At least one stepper must be " - "associated with carriage: %s" % rail.get_name()) + "At least one stepper must be " + "associated with carriage: %s" % rail.get_name() + ) steppers.update(c_steppers) for s in steppers: sk = ffi_main.gc(ffi_lib.dual_carriage_alloc(), ffi_lib.free) @@ -77,28 +89,34 @@ class DualCarriages: self.dc_stepper_kinematics.append(sk) self.orig_stepper_kinematics.append(orig_sk) s.set_stepper_kinematics(sk) + def get_axes(self): return self.axes + def get_primary_rail(self, axis): for dc_rail in self.dc_rails.values(): if dc_rail.mode == PRIMARY and dc_rail.axis == axis: return dc_rail.rail return None + def get_dc_rail_wrapper(self, rail): for dc_rail in self.dc_rails.values(): if dc_rail.rail == rail: return dc_rail return None + def get_transform(self, rail): dc_rail = self.get_dc_rail_wrapper(rail) if dc_rail is not None: return (dc_rail.scale, dc_rail.offset) - return (0., 0.) + return (0.0, 0.0) + def is_active(self, rail): dc_rail = self.get_dc_rail_wrapper(rail) return dc_rail.is_active() if dc_rail is not None else False + def toggle_active_dc_rail(self, target_dc): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() pos = toolhead.get_position() kin = toolhead.get_kinematics() @@ -107,16 +125,17 @@ class DualCarriages: if dc != target_dc and dc.axis == axis and dc.is_active(): dc.inactivate(pos) if target_dc.mode != PRIMARY: - newpos = pos[:axis] + [target_dc.get_axis_position(pos)] \ - + pos[axis+1:] + newpos = pos[:axis] + [target_dc.get_axis_position(pos)] + pos[axis + 1 :] target_dc.activate(PRIMARY, newpos, old_position=pos) toolhead.set_position(newpos) kin.update_limits(axis, target_dc.rail.get_range()) + def home(self, homing_state, axis): - kin = self.printer.lookup_object('toolhead').get_kinematics() + kin = self.printer.lookup_object("toolhead").get_kinematics() dcs = [dc for dc in self.dc_rails.values() if dc.axis == axis] - if (self.get_dc_order(dcs[0], dcs[1]) > 0) != \ - dcs[0].rail.get_homing_info().positive_dir: + if (self.get_dc_order(dcs[0], dcs[1]) > 0) != dcs[ + 0 + ].rail.get_homing_info().positive_dir: # The second carriage must home first, because the carriages home in # the same direction and the first carriage homes on the second one dcs.reverse() @@ -125,13 +144,20 @@ class DualCarriages: kin.home_axis(homing_state, axis, dc.rail) # Restore the original rails ordering self.toggle_active_dc_rail(dcs[0]) + def get_status(self, eventtime=None): - status = {'carriages' : {dc.get_name() : dc.mode - for dc in self.dc_rails.values()}} + status = { + "carriages": {dc.get_name(): dc.mode for dc in self.dc_rails.values()} + } if len(self.dc_rails) == 2: - status.update({('carriage_%d' % (i,)) : dc.mode - for i, dc in enumerate(self.dc_rails.values())}) + status.update( + { + ("carriage_%d" % (i,)): dc.mode + for i, dc in enumerate(self.dc_rails.values()) + } + ) return status + def get_kin_range(self, toolhead, mode, axis): pos = toolhead.get_position() dcs = [dc for dc in self.dc_rails.values() if dc.axis == axis] @@ -149,21 +175,19 @@ class DualCarriages: return (range_min, range_max) 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) + 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 self.get_dc_order(dcs[0], dcs[1]) > 0: - range_min = max(range_min, - 0.5 * (sum(axes_pos) + safe_dist)) - range_max = min(range_max, - sum(axes_pos) - dc1_rail.position_min) + 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) + 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: # mode == PRIMARY active_idx = 1 if dcs[1].is_active() else 0 @@ -184,6 +208,7 @@ class DualCarriages: # which actually permits carriage motion. return (range_min, range_min) return (range_min, range_max) + def get_dc_order(self, first_dc, second_dc): if first_dc == second_dc: return 0 @@ -201,8 +226,9 @@ class DualCarriages: if first_rail.position_endstop > second_rail.position_endstop: return 1 return -1 + def activate_dc_mode(self, dc, mode): - toolhead = self.printer.lookup_object('toolhead') + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() kin = toolhead.get_kinematics() axis = dc.axis @@ -214,14 +240,17 @@ class DualCarriages: self.toggle_active_dc_rail(self.get_dc_rail_wrapper(dc.dual_rail)) dc.activate(mode, toolhead.get_position()) kin.update_limits(axis, self.get_kin_range(toolhead, mode, axis)) + def _handle_ready(self): for dc_rail in self.dc_rails.values(): dc_rail.apply_transform() + cmd_SET_DUAL_CARRIAGE_help = "Configure the dual carriages mode" + def cmd_SET_DUAL_CARRIAGE(self, gcmd): - carriage_str = gcmd.get('CARRIAGE', None) + carriage_str = gcmd.get("CARRIAGE", None) if carriage_str is None: - raise gcmd.error('CARRIAGE must be specified') + raise gcmd.error("CARRIAGE must be specified") if carriage_str in self.dc_rails: dc_rail = self.dc_rails[carriage_str] else: @@ -230,65 +259,75 @@ class DualCarriages: try: index = int(carriage_str.strip()) if index < 0 or index > 1: - raise gcmd.error('Invalid CARRIAGE=%d index' % index) - dc_rail = (self.dual_rails if index - else self.primary_rails)[0] + raise gcmd.error("Invalid CARRIAGE=%d index" % index) + dc_rail = (self.dual_rails if index else self.primary_rails)[0] except ValueError: pass if dc_rail is None: - raise gcmd.error('Invalid CARRIAGE=%s specified' % carriage_str) - mode = gcmd.get('MODE', PRIMARY).upper() + raise gcmd.error("Invalid CARRIAGE=%s specified" % carriage_str) + 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 dc_rail in self.primary_rails: raise gcmd.error( - "Mode=%s is not supported for carriage=%s" % ( - mode, dc_rail.get_name())) + "Mode=%s is not supported for carriage=%s" + % (mode, dc_rail.get_name()) + ) curtime = self.printer.get_reactor().monotonic() - kin = self.printer.lookup_object('toolhead').get_kinematics() - axis = 'xyz'[dc_rail.axis] - if axis not in kin.get_status(curtime)['homed_axes']: + kin = self.printer.lookup_object("toolhead").get_kinematics() + axis = "xyz"[dc_rail.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.upper(), mode)) + "Axis %s must be homed prior to enabling mode=%s" + % (axis.upper(), mode) + ) self.activate_dc_mode(dc_rail, mode) - cmd_SAVE_DUAL_CARRIAGE_STATE_help = \ - "Save dual carriages modes and positions" + + 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') + state_name = gcmd.get("NAME", "default") self.saved_states[state_name] = self.save_dual_carriage_state() + def save_dual_carriage_state(self): - pos = self.printer.lookup_object('toolhead').get_position() - return {'carriage_modes': {dc.get_name() : dc.mode - for dc in self.dc_rails.values()}, - 'carriage_positions': {dc.get_name() : dc.get_axis_position(pos) - for dc in self.dc_rails.values()}} - cmd_RESTORE_DUAL_CARRIAGE_STATE_help = \ - "Restore dual carriages modes and positions" + pos = self.printer.lookup_object("toolhead").get_position() + return { + "carriage_modes": {dc.get_name(): dc.mode for dc in self.dc_rails.values()}, + "carriage_positions": { + dc.get_name(): dc.get_axis_position(pos) + for dc in self.dc_rails.values() + }, + } + + 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') + 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_float('MOVE_SPEED', 0., above=0.) - move = gcmd.get_int('MOVE', 1) + move_speed = gcmd.get_float("MOVE_SPEED", 0.0, above=0.0) + move = gcmd.get_int("MOVE", 1) self.restore_dual_carriage_state(saved_state, move, move_speed) - def restore_dual_carriage_state(self, saved_state, move, move_speed=0.): - toolhead = self.printer.lookup_object('toolhead') + + def restore_dual_carriage_state(self, saved_state, move, move_speed=0.0): + toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() if move: - homing_speed = 99999999. + homing_speed = 99999999.0 move_pos = list(toolhead.get_position()) cur_pos = [] - carriage_positions = saved_state['carriage_positions'] + carriage_positions = saved_state["carriage_positions"] dcs = list(self.dc_rails.values()) for dc in dcs: self.toggle_active_dc_rail(dc) homing_speed = min(homing_speed, dc.rail.homing_speed) cur_pos.append(toolhead.get_position()) - dl = [carriage_positions[dc.get_name()] - cur_pos[i][dc.axis] - for i, dc in enumerate(dcs)] + dl = [ + carriage_positions[dc.get_name()] - cur_pos[i][dc.axis] + for i, dc in enumerate(dcs) + ] for axis in self.axes: dc_ind = [i for i, dc in enumerate(dcs) if dc.axis == axis] if abs(dl[dc_ind[0]]) >= abs(dl[dc_ind[1]]): @@ -298,26 +337,29 @@ class DualCarriages: primary_dc = dcs[primary_ind] self.toggle_active_dc_rail(primary_dc) move_pos[axis] = carriage_positions[primary_dc.get_name()] - dc_mode = INACTIVE if min(abs(dl[primary_ind]), - abs(dl[secondary_ind])) < .000000001 \ - else COPY if dl[primary_ind] * dl[secondary_ind] > 0 \ - else MIRROR + dc_mode = ( + INACTIVE + if min(abs(dl[primary_ind]), abs(dl[secondary_ind])) < 0.000000001 + else COPY if dl[primary_ind] * dl[secondary_ind] > 0 else MIRROR + ) if dc_mode != INACTIVE: dcs[secondary_ind].activate(dc_mode, cur_pos[primary_ind]) dcs[secondary_ind].override_axis_scaling( - abs(dl[secondary_ind] / dl[primary_ind]), - cur_pos[primary_ind]) + abs(dl[secondary_ind] / dl[primary_ind]), cur_pos[primary_ind] + ) toolhead.manual_move(move_pos, move_speed or homing_speed) toolhead.flush_step_generation() # Make sure the scaling coefficients are restored with the mode for dc in dcs: dc.inactivate(move_pos) for dc in self.dc_rails.values(): - saved_mode = saved_state['carriage_modes'][dc.get_name()] + saved_mode = saved_state["carriage_modes"][dc.get_name()] self.activate_dc_mode(dc, saved_mode) + class DualCarriagesRail: - ENC_AXES = [b'x', b'y'] + ENC_AXES = [b"x", b"y"] + def __init__(self, printer, rail, dual_rail, axis, active): self.printer = printer self.rail = rail @@ -325,31 +367,39 @@ class DualCarriagesRail: self.sks = [s.get_stepper_kinematics() for s in rail.get_steppers()] self.axis = axis self.mode = (INACTIVE, PRIMARY)[active] - self.offset = 0. - self.scale = 1. if active else 0. + self.offset = 0.0 + self.scale = 1.0 if active else 0.0 + def get_name(self): return self.rail.get_name() + def is_active(self): 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.sks: ffi_lib.dual_carriage_set_transform( - sk, self.ENC_AXES[self.axis], self.scale, self.offset) - self.printer.send_event('dual_carriage:update_kinematics') + sk, self.ENC_AXES[self.axis], self.scale, self.offset + ) + self.printer.send_event("dual_carriage:update_kinematics") + 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.scale = -1.0 if mode == MIRROR else 1.0 self.offset = old_axis_position - position[self.axis] * self.scale self.apply_transform() self.mode = mode + def inactivate(self, position): self.offset = self.get_axis_position(position) - self.scale = 0. + self.scale = 0.0 self.apply_transform() self.mode = INACTIVE + def override_axis_scaling(self, new_scale, position): old_axis_position = self.get_axis_position(position) self.scale = math.copysign(new_scale, self.scale) |