diff options
Diffstat (limited to 'klippy/extras/force_move.py')
| -rw-r--r-- | klippy/extras/force_move.py | 141 |
1 files changed, 93 insertions, 48 deletions
diff --git a/klippy/extras/force_move.py b/klippy/extras/force_move.py index 0be0eb9b..dfdc26f2 100644 --- a/klippy/extras/force_move.py +++ b/klippy/extras/force_move.py @@ -6,20 +6,21 @@ import math, logging import chelper -BUZZ_DISTANCE = 1. -BUZZ_VELOCITY = BUZZ_DISTANCE / .250 -BUZZ_RADIANS_DISTANCE = math.radians(1.) -BUZZ_RADIANS_VELOCITY = BUZZ_RADIANS_DISTANCE / .250 +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. - if dist < 0.: - axis_r = -1. + axis_r = 1.0 + if dist < 0.0: + axis_r = -1.0 dist = -dist if not accel or not dist: - return axis_r, 0., dist / speed, speed + 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) @@ -28,6 +29,7 @@ def calc_move_time(dist, speed, accel): 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() @@ -38,112 +40,155 @@ class ForceMove: 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) + 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) + 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) + 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') + toolhead = self.printer.lookup_object("toolhead") print_time = toolhead.get_last_move_time() - stepper_enable = self.printer.lookup_object('stepper_enable') + 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 = self.printer.lookup_object("toolhead") toolhead.dwell(STALL_TIME) print_time = toolhead.get_last_move_time() - stepper_enable = self.printer.lookup_object('stepper_enable') + 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.): - toolhead = self.printer.lookup_object('toolhead') + + 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.)) + 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., axis_r, 0., 0., 0., cruise_v, accel) + 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) + 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') + 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') + 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(.050) + toolhead.dwell(0.050) self.manual_move(stepper, -dist, speed) - toolhead.dwell(.450) + 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.) - accel = gcmd.get_float('ACCEL', 0., minval=0.) - logging.info("FORCE_MOVE %s distance=%.3f velocity=%.3f accel=%.3f", - stepper.get_name(), distance, speed, accel) + 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 = 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() + 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: + if gcmd.get("CLEAR_HOMED", None) is None: # "CLEAR" is an alias for "CLEAR_HOMED"; should deprecate - clear_homed = gcmd.get('CLEAR', '').lower() + clear_homed = gcmd.get("CLEAR", "").lower() else: - clear_homed = gcmd.get('CLEAR_HOMED', '').lower() + 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) + 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) |