mirror of
https://github.com/Klipper3d/klipper.git
synced 2025-10-31 02:15:52 +01:00
218 lines
10 KiB
Python
218 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
|
|
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)
|
|
self.motion_queuing.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()
|
|
start_time = self.next_cmd_time
|
|
end_time = self._submit_move(start_time, newpos[0],
|
|
speed, self.homing_accel)
|
|
# Drip updates to motors
|
|
self.motion_queuing.drip_update_time(start_time, end_time,
|
|
drip_completion)
|
|
# Clear trapq of any remaining parts of movement
|
|
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)
|