mirror of
https://github.com/andreili/klipper.git
synced 2025-09-15 09:51:12 +02:00
5056e1031c
Invoke flush_step_generation() prior to checking motor enable state as this is the best way to ensure all stepper active callbacks have been invoked (which could change the enable line state). Also, there is no longer a reason to add additional toolhead dwells when enabling a stepper motor. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
219 lines
10 KiB
Python
219 lines
10 KiB
Python
# Support for a manual controlled stepper
|
|
#
|
|
# Copyright (C) 2019-2025 Kevin O'Connor <kevin@koconnor.net>
|
|
#
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
|
import logging
|
|
import stepper, chelper
|
|
from . import force_move
|
|
|
|
class ManualStepper:
|
|
def __init__(self, config):
|
|
self.printer = config.get_printer()
|
|
if config.get('endstop_pin', None) is not None:
|
|
self.can_home = True
|
|
self.rail = stepper.LookupRail(
|
|
config, need_position_minmax=False, default_position_endstop=0.)
|
|
self.steppers = self.rail.get_steppers()
|
|
else:
|
|
self.can_home = False
|
|
self.rail = stepper.PrinterStepper(config)
|
|
self.steppers = [self.rail]
|
|
self.velocity = config.getfloat('velocity', 5., above=0.)
|
|
self.accel = self.homing_accel = config.getfloat('accel', 0., minval=0.)
|
|
self.next_cmd_time = 0.
|
|
self.commanded_pos = 0.
|
|
self.pos_min = config.getfloat('position_min', None)
|
|
self.pos_max = config.getfloat('position_max', None)
|
|
# Setup iterative solver
|
|
self.motion_queuing = self.printer.load_object(config, 'motion_queuing')
|
|
self.trapq = self.motion_queuing.allocate_trapq()
|
|
self.trapq_append = self.motion_queuing.lookup_trapq_append()
|
|
self.rail.setup_itersolve('cartesian_stepper_alloc', b'x')
|
|
self.rail.set_trapq(self.trapq)
|
|
# Registered with toolhead as an axtra axis
|
|
self.axis_gcode_id = None
|
|
self.instant_corner_v = 0.
|
|
self.gaxis_limit_velocity = self.gaxis_limit_accel = 0.
|
|
# Register commands
|
|
stepper_name = config.get_name().split()[1]
|
|
gcode = self.printer.lookup_object('gcode')
|
|
gcode.register_mux_command('MANUAL_STEPPER', "STEPPER",
|
|
stepper_name, self.cmd_MANUAL_STEPPER,
|
|
desc=self.cmd_MANUAL_STEPPER_help)
|
|
def sync_print_time(self):
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
print_time = toolhead.get_last_move_time()
|
|
if self.next_cmd_time > print_time:
|
|
toolhead.dwell(self.next_cmd_time - print_time)
|
|
else:
|
|
self.next_cmd_time = print_time
|
|
def do_enable(self, enable):
|
|
stepper_names = [s.get_name() for s in self.steppers]
|
|
stepper_enable = self.printer.lookup_object('stepper_enable')
|
|
stepper_enable.set_motors_enable(stepper_names, enable)
|
|
def do_set_position(self, setpos):
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
toolhead.flush_step_generation()
|
|
self.commanded_pos = setpos
|
|
self.rail.set_position([self.commanded_pos, 0., 0.])
|
|
def _submit_move(self, movetime, movepos, speed, accel):
|
|
cp = self.commanded_pos
|
|
dist = movepos - cp
|
|
axis_r, accel_t, cruise_t, cruise_v = force_move.calc_move_time(
|
|
dist, speed, accel)
|
|
self.trapq_append(self.trapq, movetime,
|
|
accel_t, cruise_t, accel_t,
|
|
cp, 0., 0., axis_r, 0., 0.,
|
|
0., cruise_v, accel)
|
|
self.commanded_pos = movepos
|
|
return movetime + accel_t + cruise_t + accel_t
|
|
def do_move(self, movepos, speed, accel, sync=True):
|
|
self.sync_print_time()
|
|
self.next_cmd_time = self._submit_move(self.next_cmd_time, movepos,
|
|
speed, accel)
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
toolhead.note_mcu_movequeue_activity(self.next_cmd_time)
|
|
if sync:
|
|
self.sync_print_time()
|
|
def do_homing_move(self, movepos, speed, accel, triggered, check_trigger):
|
|
if not self.can_home:
|
|
raise self.printer.command_error(
|
|
"No endstop for this manual stepper")
|
|
self.homing_accel = accel
|
|
pos = [movepos, 0., 0., 0.]
|
|
endstops = self.rail.get_endstops()
|
|
phoming = self.printer.lookup_object('homing')
|
|
phoming.manual_home(self, endstops, pos, speed,
|
|
triggered, check_trigger)
|
|
cmd_MANUAL_STEPPER_help = "Command a manually configured stepper"
|
|
def cmd_MANUAL_STEPPER(self, gcmd):
|
|
if gcmd.get('GCODE_AXIS', None) is not None:
|
|
return self.command_with_gcode_axis(gcmd)
|
|
if self.axis_gcode_id is not None:
|
|
raise gcmd.error("Must unregister from gcode axis first")
|
|
enable = gcmd.get_int('ENABLE', None)
|
|
if enable is not None:
|
|
self.do_enable(enable)
|
|
setpos = gcmd.get_float('SET_POSITION', None)
|
|
if setpos is not None:
|
|
self.do_set_position(setpos)
|
|
speed = gcmd.get_float('SPEED', self.velocity, above=0.)
|
|
accel = gcmd.get_float('ACCEL', self.accel, minval=0.)
|
|
homing_move = gcmd.get_int('STOP_ON_ENDSTOP', 0)
|
|
if homing_move:
|
|
movepos = gcmd.get_float('MOVE')
|
|
if ((self.pos_min is not None and movepos < self.pos_min)
|
|
or (self.pos_max is not None and movepos > self.pos_max)):
|
|
raise gcmd.error("Move out of range")
|
|
self.do_homing_move(movepos, speed, accel,
|
|
homing_move > 0, abs(homing_move) == 1)
|
|
elif gcmd.get_float('MOVE', None) is not None:
|
|
movepos = gcmd.get_float('MOVE')
|
|
if ((self.pos_min is not None and movepos < self.pos_min)
|
|
or (self.pos_max is not None and movepos > self.pos_max)):
|
|
raise gcmd.error("Move out of range")
|
|
sync = gcmd.get_int('SYNC', 1)
|
|
self.do_move(movepos, speed, accel, sync)
|
|
elif gcmd.get_int('SYNC', 0):
|
|
self.sync_print_time()
|
|
# Register as a gcode axis
|
|
def command_with_gcode_axis(self, gcmd):
|
|
gcode_move = self.printer.lookup_object("gcode_move")
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
gcode_axis = gcmd.get('GCODE_AXIS').upper()
|
|
instant_corner_v = gcmd.get_float('INSTANTANEOUS_CORNER_VELOCITY', 1.,
|
|
minval=0.)
|
|
limit_velocity = gcmd.get_float('LIMIT_VELOCITY', 999999.9, above=0.)
|
|
limit_accel = gcmd.get_float('LIMIT_ACCEL', 999999.9, above=0.)
|
|
if self.axis_gcode_id is not None:
|
|
if gcode_axis:
|
|
raise gcmd.error("Must unregister axis first")
|
|
# Unregister
|
|
toolhead.remove_extra_axis(self)
|
|
self.axis_gcode_id = None
|
|
return
|
|
if (len(gcode_axis) != 1 or not gcode_axis.isupper()
|
|
or gcode_axis in "XYZEFN"):
|
|
if not gcode_axis:
|
|
# Request to unregister already unregistered axis
|
|
return
|
|
raise gcmd.error("Not a valid GCODE_AXIS")
|
|
for ea in toolhead.get_extra_axes():
|
|
if ea is not None and ea.get_axis_gcode_id() == gcode_axis:
|
|
raise gcmd.error("Axis '%s' already registered" % (gcode_axis,))
|
|
self.axis_gcode_id = gcode_axis
|
|
self.instant_corner_v = instant_corner_v
|
|
self.gaxis_limit_velocity = limit_velocity
|
|
self.gaxis_limit_accel = limit_accel
|
|
toolhead.add_extra_axis(self, self.commanded_pos)
|
|
def process_move(self, print_time, move, ea_index):
|
|
axis_r = move.axes_r[ea_index]
|
|
start_pos = move.start_pos[ea_index]
|
|
accel = move.accel * axis_r
|
|
start_v = move.start_v * axis_r
|
|
cruise_v = move.cruise_v * axis_r
|
|
self.trapq_append(self.trapq, print_time,
|
|
move.accel_t, move.cruise_t, move.decel_t,
|
|
start_pos, 0., 0.,
|
|
1., 0., 0.,
|
|
start_v, cruise_v, accel)
|
|
self.commanded_pos = move.end_pos[ea_index]
|
|
def check_move(self, move, ea_index):
|
|
# Check move is in bounds
|
|
movepos = move.end_pos[ea_index]
|
|
if ((self.pos_min is not None and movepos < self.pos_min)
|
|
or (self.pos_max is not None and movepos > self.pos_max)):
|
|
raise move.move_error()
|
|
# Check if need to limit maximum velocity and acceleration
|
|
axis_ratio = move.move_d / abs(move.axes_d[ea_index])
|
|
limit_velocity = self.gaxis_limit_velocity * axis_ratio
|
|
limit_accel = self.gaxis_limit_accel * axis_ratio
|
|
if not move.is_kinematic_move and self.accel:
|
|
limit_accel = min(limit_accel, self.accel * axis_ratio)
|
|
move.limit_speed(limit_velocity, limit_accel)
|
|
def calc_junction(self, prev_move, move, ea_index):
|
|
diff_r = move.axes_r[ea_index] - prev_move.axes_r[ea_index]
|
|
if diff_r:
|
|
return (self.instant_corner_v / abs(diff_r))**2
|
|
return move.max_cruise_v2
|
|
def get_axis_gcode_id(self):
|
|
return self.axis_gcode_id
|
|
def get_trapq(self):
|
|
return self.trapq
|
|
# Toolhead wrappers to support homing
|
|
def flush_step_generation(self):
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
toolhead.flush_step_generation()
|
|
def get_position(self):
|
|
return [self.commanded_pos, 0., 0., 0.]
|
|
def set_position(self, newpos, homing_axes=""):
|
|
self.do_set_position(newpos[0])
|
|
def get_last_move_time(self):
|
|
self.sync_print_time()
|
|
return self.next_cmd_time
|
|
def dwell(self, delay):
|
|
self.next_cmd_time += max(0., delay)
|
|
def drip_move(self, newpos, speed, drip_completion):
|
|
# Submit move to trapq
|
|
self.sync_print_time()
|
|
maxtime = self._submit_move(self.next_cmd_time, newpos[0],
|
|
speed, self.homing_accel)
|
|
# Drip updates to motors
|
|
toolhead = self.printer.lookup_object('toolhead')
|
|
toolhead.drip_update_time(maxtime, drip_completion)
|
|
# Clear trapq of any remaining parts of movement
|
|
reactor = self.printer.get_reactor()
|
|
self.motion_queuing.wipe_trapq(self.trapq)
|
|
self.rail.set_position([self.commanded_pos, 0., 0.])
|
|
self.sync_print_time()
|
|
def get_kinematics(self):
|
|
return self
|
|
def get_steppers(self):
|
|
return self.steppers
|
|
def calc_position(self, stepper_positions):
|
|
return [stepper_positions[self.rail.get_name()], 0., 0.]
|
|
|
|
def load_config_prefix(config):
|
|
return ManualStepper(config)
|