138 lines
6.2 KiB
Python
138 lines
6.2 KiB
Python
# Utility for manually moving a stepper for diagnostic purposes
|
|
#
|
|
# Copyright (C) 2018-2019 Kevin O'Connor <kevin@koconnor.net>
|
|
#
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
|
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
|
|
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.
|
|
dist = -dist
|
|
if not accel or not dist:
|
|
return axis_r, 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.):
|
|
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.))
|
|
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)
|
|
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.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(.050)
|
|
self.manual_move(stepper, -dist, speed)
|
|
toolhead.dwell(.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)
|
|
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])
|
|
logging.info("SET_KINEMATIC_POSITION pos=%.3f,%.3f,%.3f", x, y, z)
|
|
toolhead.set_position([x, y, z, curpos[3]], homing_axes=(0, 1, 2))
|
|
|
|
def load_config(config):
|
|
return ForceMove(config)
|