# Utility for manually moving a stepper for diagnostic purposes # # Copyright (C) 2018-2019 Kevin O'Connor # # This file may be distributed under the terms of the GNU GPLv3 license. import logging import math import chelper BUZZ_DISTANCE = 1.0 BUZZ_VELOCITY = BUZZ_DISTANCE / 0.250 BUZZ_RADIANS_DISTANCE = math.radians(1.0) BUZZ_RADIANS_VELOCITY = BUZZ_RADIANS_DISTANCE / 0.250 STALL_TIME = 0.100 # Calculate a move's accel_t, cruise_t, and cruise_v def calc_move_time(dist, speed, accel): axis_r = 1.0 if dist < 0.0: axis_r = -1.0 dist = -dist if not accel or not dist: return axis_r, 0.0, dist / speed, speed max_cruise_v2 = dist * accel if max_cruise_v2 < speed**2: speed = math.sqrt(max_cruise_v2) accel_t = speed / accel accel_decel_d = accel_t * speed cruise_t = (dist - accel_decel_d) / speed return axis_r, accel_t, cruise_t, speed class ForceMove: def __init__(self, config): self.printer = config.get_printer() self.steppers = {} # Setup iterative solver ffi_main, ffi_lib = chelper.get_ffi() self.trapq = ffi_main.gc(ffi_lib.trapq_alloc(), ffi_lib.trapq_free) self.trapq_append = ffi_lib.trapq_append self.trapq_finalize_moves = ffi_lib.trapq_finalize_moves self.stepper_kinematics = ffi_main.gc( ffi_lib.cartesian_stepper_alloc(b"x"), ffi_lib.free ) # Register commands gcode = self.printer.lookup_object("gcode") gcode.register_command( "STEPPER_BUZZ", self.cmd_STEPPER_BUZZ, desc=self.cmd_STEPPER_BUZZ_help ) if config.getboolean("enable_force_move", False): gcode.register_command( "FORCE_MOVE", self.cmd_FORCE_MOVE, desc=self.cmd_FORCE_MOVE_help ) gcode.register_command( "SET_KINEMATIC_POSITION", self.cmd_SET_KINEMATIC_POSITION, desc=self.cmd_SET_KINEMATIC_POSITION_help, ) def register_stepper(self, config, mcu_stepper): self.steppers[mcu_stepper.get_name()] = mcu_stepper def lookup_stepper(self, name): if name not in self.steppers: raise self.printer.config_error("Unknown stepper %s" % (name,)) return self.steppers[name] def _force_enable(self, stepper): toolhead = self.printer.lookup_object("toolhead") print_time = toolhead.get_last_move_time() stepper_enable = self.printer.lookup_object("stepper_enable") enable = stepper_enable.lookup_enable(stepper.get_name()) was_enable = enable.is_motor_enabled() if not was_enable: enable.motor_enable(print_time) toolhead.dwell(STALL_TIME) return was_enable def _restore_enable(self, stepper, was_enable): if not was_enable: toolhead = self.printer.lookup_object("toolhead") toolhead.dwell(STALL_TIME) print_time = toolhead.get_last_move_time() stepper_enable = self.printer.lookup_object("stepper_enable") enable = stepper_enable.lookup_enable(stepper.get_name()) enable.motor_disable(print_time) toolhead.dwell(STALL_TIME) def manual_move(self, stepper, dist, speed, accel=0.0): toolhead = self.printer.lookup_object("toolhead") toolhead.flush_step_generation() prev_sk = stepper.set_stepper_kinematics(self.stepper_kinematics) prev_trapq = stepper.set_trapq(self.trapq) stepper.set_position((0.0, 0.0, 0.0)) axis_r, accel_t, cruise_t, cruise_v = calc_move_time(dist, speed, accel) print_time = toolhead.get_last_move_time() self.trapq_append( self.trapq, print_time, accel_t, cruise_t, accel_t, 0.0, 0.0, 0.0, axis_r, 0.0, 0.0, 0.0, cruise_v, accel, ) print_time = print_time + accel_t + cruise_t + accel_t stepper.generate_steps(print_time) self.trapq_finalize_moves( self.trapq, print_time + 99999.9, print_time + 99999.9 ) stepper.set_trapq(prev_trapq) stepper.set_stepper_kinematics(prev_sk) toolhead.note_mcu_movequeue_activity(print_time) toolhead.dwell(accel_t + cruise_t + accel_t) toolhead.flush_step_generation() def _lookup_stepper(self, gcmd): name = gcmd.get("STEPPER") if name not in self.steppers: raise gcmd.error("Unknown stepper %s" % (name,)) return self.steppers[name] cmd_STEPPER_BUZZ_help = "Oscillate a given stepper to help id it" def cmd_STEPPER_BUZZ(self, gcmd): stepper = self._lookup_stepper(gcmd) logging.info("Stepper buzz %s", stepper.get_name()) was_enable = self._force_enable(stepper) toolhead = self.printer.lookup_object("toolhead") dist, speed = BUZZ_DISTANCE, BUZZ_VELOCITY if stepper.units_in_radians(): dist, speed = BUZZ_RADIANS_DISTANCE, BUZZ_RADIANS_VELOCITY for i in range(10): self.manual_move(stepper, dist, speed) toolhead.dwell(0.050) self.manual_move(stepper, -dist, speed) toolhead.dwell(0.450) self._restore_enable(stepper, was_enable) cmd_FORCE_MOVE_help = "Manually move a stepper; invalidates kinematics" def cmd_FORCE_MOVE(self, gcmd): stepper = self._lookup_stepper(gcmd) distance = gcmd.get_float("DISTANCE") speed = gcmd.get_float("VELOCITY", above=0.0) accel = gcmd.get_float("ACCEL", 0.0, minval=0.0) logging.info( "FORCE_MOVE %s distance=%.3f velocity=%.3f accel=%.3f", stepper.get_name(), distance, speed, accel, ) self._force_enable(stepper) self.manual_move(stepper, distance, speed, accel) cmd_SET_KINEMATIC_POSITION_help = "Force a low-level kinematic position" def cmd_SET_KINEMATIC_POSITION(self, gcmd): toolhead = self.printer.lookup_object("toolhead") toolhead.get_last_move_time() curpos = toolhead.get_position() x = gcmd.get_float("X", curpos[0]) y = gcmd.get_float("Y", curpos[1]) z = gcmd.get_float("Z", curpos[2]) set_homed = gcmd.get("SET_HOMED", "xyz").lower() set_homed_axes = "".join([a for a in "xyz" if a in set_homed]) if gcmd.get("CLEAR_HOMED", None) is None: # "CLEAR" is an alias for "CLEAR_HOMED"; should deprecate clear_homed = gcmd.get("CLEAR", "").lower() else: clear_homed = gcmd.get("CLEAR_HOMED", "").lower() clear_homed_axes = "".join([a for a in "xyz" if a in clear_homed]) logging.info( "SET_KINEMATIC_POSITION pos=%.3f,%.3f,%.3f" " set_homed=%s clear_homed=%s", x, y, z, set_homed_axes, clear_homed_axes, ) toolhead.set_position([x, y, z], homing_axes=set_homed_axes) toolhead.get_kinematics().clear_homing_state(clear_homed_axes) def load_config(config): return ForceMove(config)