mirror of https://github.com/Desuuuu/klipper.git
469 lines
22 KiB
Python
469 lines
22 KiB
Python
# Code for coordinating events on the printer toolhead
|
|
#
|
|
# Copyright (C) 2016-2018 Kevin O'Connor <kevin@koconnor.net>
|
|
#
|
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
|
import math, logging, importlib
|
|
import mcu, homing, chelper, kinematics.extruder
|
|
|
|
# Common suffixes: _d is distance (in mm), _v is velocity (in
|
|
# mm/second), _v2 is velocity squared (mm^2/s^2), _t is time (in
|
|
# seconds), _r is ratio (scalar between 0.0 and 1.0)
|
|
|
|
# Class to track each move request
|
|
class Move:
|
|
def __init__(self, toolhead, start_pos, end_pos, speed):
|
|
self.toolhead = toolhead
|
|
self.start_pos = tuple(start_pos)
|
|
self.end_pos = tuple(end_pos)
|
|
self.accel = toolhead.max_accel
|
|
self.cmove = toolhead.cmove
|
|
self.is_kinematic_move = True
|
|
self.axes_d = axes_d = [end_pos[i] - start_pos[i] for i in (0, 1, 2, 3)]
|
|
self.move_d = move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
|
|
if move_d < .000000001:
|
|
# Extrude only move
|
|
self.end_pos = (start_pos[0], start_pos[1], start_pos[2],
|
|
end_pos[3])
|
|
axes_d[0] = axes_d[1] = axes_d[2] = 0.
|
|
self.move_d = move_d = abs(axes_d[3])
|
|
self.is_kinematic_move = False
|
|
self.min_move_t = move_d / speed
|
|
# Junction speeds are tracked in velocity squared. The
|
|
# delta_v2 is the maximum amount of this squared-velocity that
|
|
# can change in this move.
|
|
self.max_start_v2 = 0.
|
|
self.max_cruise_v2 = speed**2
|
|
self.delta_v2 = 2.0 * move_d * self.accel
|
|
self.max_smoothed_v2 = 0.
|
|
self.smooth_delta_v2 = 2.0 * move_d * toolhead.max_accel_to_decel
|
|
def limit_speed(self, speed, accel):
|
|
speed2 = speed**2
|
|
if speed2 < self.max_cruise_v2:
|
|
self.max_cruise_v2 = speed2
|
|
self.min_move_t = self.move_d / speed
|
|
self.accel = min(self.accel, accel)
|
|
self.delta_v2 = 2.0 * self.move_d * self.accel
|
|
self.smooth_delta_v2 = min(self.smooth_delta_v2, self.delta_v2)
|
|
def calc_junction(self, prev_move):
|
|
if not self.is_kinematic_move or not prev_move.is_kinematic_move:
|
|
return
|
|
# Allow extruder to calculate its maximum junction
|
|
extruder_v2 = self.toolhead.extruder.calc_junction(prev_move, self)
|
|
# Find max velocity using approximated centripetal velocity as
|
|
# described at:
|
|
# https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
|
|
axes_d = self.axes_d
|
|
prev_axes_d = prev_move.axes_d
|
|
junction_cos_theta = -((axes_d[0] * prev_axes_d[0]
|
|
+ axes_d[1] * prev_axes_d[1]
|
|
+ axes_d[2] * prev_axes_d[2])
|
|
/ (self.move_d * prev_move.move_d))
|
|
if junction_cos_theta > 0.999999:
|
|
return
|
|
junction_cos_theta = max(junction_cos_theta, -0.999999)
|
|
sin_theta_d2 = math.sqrt(0.5*(1.0-junction_cos_theta))
|
|
R = self.toolhead.junction_deviation * sin_theta_d2 / (1. - sin_theta_d2)
|
|
tan_theta_d2 = sin_theta_d2 / math.sqrt(0.5*(1.0+junction_cos_theta))
|
|
move_centripetal_v2 = .5 * self.move_d * tan_theta_d2 * self.accel
|
|
prev_move_centripetal_v2 = (.5 * prev_move.move_d * tan_theta_d2
|
|
* prev_move.accel)
|
|
self.max_start_v2 = min(
|
|
R * self.accel, R * prev_move.accel,
|
|
move_centripetal_v2, prev_move_centripetal_v2,
|
|
extruder_v2, self.max_cruise_v2, prev_move.max_cruise_v2,
|
|
prev_move.max_start_v2 + prev_move.delta_v2)
|
|
self.max_smoothed_v2 = min(
|
|
self.max_start_v2
|
|
, prev_move.max_smoothed_v2 + prev_move.smooth_delta_v2)
|
|
def set_junction(self, start_v2, cruise_v2, end_v2):
|
|
# Determine accel, cruise, and decel portions of the move distance
|
|
inv_delta_v2 = 1. / self.delta_v2
|
|
self.accel_r = accel_r = (cruise_v2 - start_v2) * inv_delta_v2
|
|
self.decel_r = decel_r = (cruise_v2 - end_v2) * inv_delta_v2
|
|
self.cruise_r = cruise_r = 1. - accel_r - decel_r
|
|
# Determine move velocities
|
|
self.start_v = start_v = math.sqrt(start_v2)
|
|
self.cruise_v = cruise_v = math.sqrt(cruise_v2)
|
|
self.end_v = end_v = math.sqrt(end_v2)
|
|
# Determine time spent in each portion of move (time is the
|
|
# distance divided by average velocity)
|
|
self.accel_t = accel_r * self.move_d / ((start_v + cruise_v) * 0.5)
|
|
self.cruise_t = cruise_r * self.move_d / cruise_v
|
|
self.decel_t = decel_r * self.move_d / ((end_v + cruise_v) * 0.5)
|
|
def move(self):
|
|
# Generate step times for the move
|
|
next_move_time = self.toolhead.get_next_move_time()
|
|
if self.is_kinematic_move:
|
|
self.toolhead.move_fill(
|
|
self.cmove, next_move_time,
|
|
self.accel_t, self.cruise_t, self.decel_t,
|
|
self.start_pos[0], self.start_pos[1], self.start_pos[2],
|
|
self.axes_d[0], self.axes_d[1], self.axes_d[2],
|
|
self.start_v, self.cruise_v, self.accel)
|
|
self.toolhead.kin.move(next_move_time, self)
|
|
if self.axes_d[3]:
|
|
self.toolhead.extruder.move(next_move_time, self)
|
|
self.toolhead.update_move_time(
|
|
self.accel_t + self.cruise_t + self.decel_t)
|
|
|
|
LOOKAHEAD_FLUSH_TIME = 0.250
|
|
|
|
# Class to track a list of pending move requests and to facilitate
|
|
# "look-ahead" across moves to reduce acceleration between moves.
|
|
class MoveQueue:
|
|
def __init__(self):
|
|
self.extruder_lookahead = None
|
|
self.queue = []
|
|
self.leftover = 0
|
|
self.junction_flush = LOOKAHEAD_FLUSH_TIME
|
|
def reset(self):
|
|
del self.queue[:]
|
|
self.leftover = 0
|
|
self.junction_flush = LOOKAHEAD_FLUSH_TIME
|
|
def set_flush_time(self, flush_time):
|
|
self.junction_flush = flush_time
|
|
def set_extruder(self, extruder):
|
|
self.extruder_lookahead = extruder.lookahead
|
|
def flush(self, lazy=False):
|
|
self.junction_flush = LOOKAHEAD_FLUSH_TIME
|
|
update_flush_count = lazy
|
|
queue = self.queue
|
|
flush_count = len(queue)
|
|
# Traverse queue from last to first move and determine maximum
|
|
# junction speed assuming the robot comes to a complete stop
|
|
# after the last move.
|
|
delayed = []
|
|
next_end_v2 = next_smoothed_v2 = peak_cruise_v2 = 0.
|
|
for i in range(flush_count-1, self.leftover-1, -1):
|
|
move = queue[i]
|
|
reachable_start_v2 = next_end_v2 + move.delta_v2
|
|
start_v2 = min(move.max_start_v2, reachable_start_v2)
|
|
reachable_smoothed_v2 = next_smoothed_v2 + move.smooth_delta_v2
|
|
smoothed_v2 = min(move.max_smoothed_v2, reachable_smoothed_v2)
|
|
if smoothed_v2 < reachable_smoothed_v2:
|
|
# It's possible for this move to accelerate
|
|
if (smoothed_v2 + move.smooth_delta_v2 > next_smoothed_v2
|
|
or delayed):
|
|
# This move can decelerate or this is a full accel
|
|
# move after a full decel move
|
|
if update_flush_count and peak_cruise_v2:
|
|
flush_count = i
|
|
update_flush_count = False
|
|
peak_cruise_v2 = min(move.max_cruise_v2, (
|
|
smoothed_v2 + reachable_smoothed_v2) * .5)
|
|
if delayed:
|
|
# Propagate peak_cruise_v2 to any delayed moves
|
|
if not update_flush_count and i < flush_count:
|
|
for m, ms_v2, me_v2 in delayed:
|
|
mc_v2 = min(peak_cruise_v2, ms_v2)
|
|
m.set_junction(min(ms_v2, mc_v2), mc_v2
|
|
, min(me_v2, mc_v2))
|
|
del delayed[:]
|
|
if not update_flush_count and i < flush_count:
|
|
cruise_v2 = min((start_v2 + reachable_start_v2) * .5
|
|
, move.max_cruise_v2, peak_cruise_v2)
|
|
move.set_junction(min(start_v2, cruise_v2), cruise_v2
|
|
, min(next_end_v2, cruise_v2))
|
|
else:
|
|
# Delay calculating this move until peak_cruise_v2 is known
|
|
delayed.append((move, start_v2, next_end_v2))
|
|
next_end_v2 = start_v2
|
|
next_smoothed_v2 = smoothed_v2
|
|
if update_flush_count:
|
|
return
|
|
# Allow extruder to do its lookahead
|
|
move_count = self.extruder_lookahead(queue, flush_count, lazy)
|
|
# Generate step times for all moves ready to be flushed
|
|
for move in queue[:move_count]:
|
|
move.move()
|
|
# Remove processed moves from the queue
|
|
self.leftover = flush_count - move_count
|
|
del queue[:move_count]
|
|
def add_move(self, move):
|
|
self.queue.append(move)
|
|
if len(self.queue) == 1:
|
|
return
|
|
move.calc_junction(self.queue[-2])
|
|
self.junction_flush -= move.min_move_t
|
|
if self.junction_flush <= 0.:
|
|
# There are enough queued moves to return to zero velocity
|
|
# from the first move's maximum possible velocity, so at
|
|
# least one move can be flushed.
|
|
self.flush(lazy=True)
|
|
|
|
STALL_TIME = 0.100
|
|
|
|
# Main code to track events (and their timing) on the printer toolhead
|
|
class ToolHead:
|
|
def __init__(self, config):
|
|
self.printer = config.get_printer()
|
|
self.reactor = self.printer.get_reactor()
|
|
self.all_mcus = [
|
|
m for n, m in self.printer.lookup_objects(module='mcu')]
|
|
self.mcu = self.all_mcus[0]
|
|
self.move_queue = MoveQueue()
|
|
self.commanded_pos = [0., 0., 0., 0.]
|
|
# Velocity and acceleration control
|
|
self.max_velocity = config.getfloat('max_velocity', above=0.)
|
|
self.max_accel = config.getfloat('max_accel', above=0.)
|
|
self.requested_accel_to_decel = config.getfloat(
|
|
'max_accel_to_decel', self.max_accel * 0.5, above=0.)
|
|
self.max_accel_to_decel = min(self.requested_accel_to_decel,
|
|
self.max_accel)
|
|
self.square_corner_velocity = config.getfloat(
|
|
'square_corner_velocity', 5., minval=0.)
|
|
self.config_max_velocity = self.max_velocity
|
|
self.config_max_accel = self.max_accel
|
|
self.config_square_corner_velocity = self.square_corner_velocity
|
|
self.junction_deviation = 0.
|
|
self._calc_junction_deviation()
|
|
# Print time tracking
|
|
self.buffer_time_low = config.getfloat(
|
|
'buffer_time_low', 1.000, above=0.)
|
|
self.buffer_time_high = config.getfloat(
|
|
'buffer_time_high', 2.000, above=self.buffer_time_low)
|
|
self.buffer_time_start = config.getfloat(
|
|
'buffer_time_start', 0.250, above=0.)
|
|
self.move_flush_time = config.getfloat(
|
|
'move_flush_time', 0.050, above=0.)
|
|
self.print_time = 0.
|
|
self.last_print_start_time = 0.
|
|
self.need_check_stall = -1.
|
|
self.print_stall = 0
|
|
self.sync_print_time = True
|
|
self.idle_flush_print_time = 0.
|
|
self.flush_timer = self.reactor.register_timer(self._flush_handler)
|
|
self.move_queue.set_flush_time(self.buffer_time_high)
|
|
self.printer.try_load_module(config, "idle_timeout")
|
|
self.printer.try_load_module(config, "statistics")
|
|
# Setup iterative solver
|
|
ffi_main, ffi_lib = chelper.get_ffi()
|
|
self.cmove = ffi_main.gc(ffi_lib.move_alloc(), ffi_lib.free)
|
|
self.move_fill = ffi_lib.move_fill
|
|
# Create kinematics class
|
|
self.extruder = kinematics.extruder.DummyExtruder()
|
|
self.move_queue.set_extruder(self.extruder)
|
|
kin_name = config.get('kinematics')
|
|
try:
|
|
mod = importlib.import_module('kinematics.' + kin_name)
|
|
self.kin = mod.load_kinematics(self, config)
|
|
except config.error as e:
|
|
raise
|
|
except self.printer.lookup_object('pins').error as e:
|
|
raise
|
|
except:
|
|
msg = "Error loading kinematics '%s'" % (kin_name,)
|
|
logging.exception(msg)
|
|
raise config.error(msg)
|
|
# SET_VELOCITY_LIMIT command
|
|
gcode = self.printer.lookup_object('gcode')
|
|
gcode.register_command('SET_VELOCITY_LIMIT', self.cmd_SET_VELOCITY_LIMIT,
|
|
desc=self.cmd_SET_VELOCITY_LIMIT_help)
|
|
gcode.register_command('M204', self.cmd_M204)
|
|
# Print time tracking
|
|
def update_move_time(self, movetime):
|
|
self.print_time += movetime
|
|
flush_to_time = self.print_time - self.move_flush_time
|
|
for m in self.all_mcus:
|
|
m.flush_moves(flush_to_time)
|
|
def get_next_move_time(self):
|
|
if not self.sync_print_time:
|
|
return self.print_time
|
|
self.sync_print_time = False
|
|
est_print_time = self.mcu.estimated_print_time(self.reactor.monotonic())
|
|
if est_print_time + self.buffer_time_start > self.print_time:
|
|
self.print_time = est_print_time + self.buffer_time_start
|
|
self.last_print_start_time = self.print_time
|
|
self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
|
|
return self.print_time
|
|
def _flush_lookahead(self, must_sync=False):
|
|
sync_print_time = self.sync_print_time
|
|
self.move_queue.flush()
|
|
self.idle_flush_print_time = 0.
|
|
if sync_print_time or must_sync:
|
|
self.sync_print_time = True
|
|
self.move_queue.set_flush_time(self.buffer_time_high)
|
|
self.need_check_stall = -1.
|
|
self.reactor.update_timer(self.flush_timer, self.reactor.NEVER)
|
|
for m in self.all_mcus:
|
|
m.flush_moves(self.print_time)
|
|
def get_last_move_time(self):
|
|
self._flush_lookahead()
|
|
return self.get_next_move_time()
|
|
def reset_print_time(self, min_print_time=0.):
|
|
self._flush_lookahead(must_sync=True)
|
|
self.print_time = max(min_print_time, self.mcu.estimated_print_time(
|
|
self.reactor.monotonic()))
|
|
def _check_stall(self):
|
|
eventtime = self.reactor.monotonic()
|
|
if self.sync_print_time:
|
|
# Building initial queue - make sure to flush on idle input
|
|
if self.idle_flush_print_time:
|
|
est_print_time = self.mcu.estimated_print_time(eventtime)
|
|
if est_print_time < self.idle_flush_print_time:
|
|
self.print_stall += 1
|
|
self.idle_flush_print_time = 0.
|
|
self.reactor.update_timer(self.flush_timer, eventtime + 0.100)
|
|
return
|
|
# Check if there are lots of queued moves and stall if so
|
|
while 1:
|
|
est_print_time = self.mcu.estimated_print_time(eventtime)
|
|
buffer_time = self.print_time - est_print_time
|
|
stall_time = buffer_time - self.buffer_time_high
|
|
if stall_time <= 0.:
|
|
break
|
|
if self.mcu.is_fileoutput():
|
|
self.need_check_stall = self.reactor.NEVER
|
|
return
|
|
eventtime = self.reactor.pause(eventtime + min(1., stall_time))
|
|
self.need_check_stall = est_print_time + self.buffer_time_high + 0.100
|
|
def _flush_handler(self, eventtime):
|
|
try:
|
|
print_time = self.print_time
|
|
buffer_time = print_time - self.mcu.estimated_print_time(eventtime)
|
|
if buffer_time > self.buffer_time_low:
|
|
# Running normally - reschedule check
|
|
return eventtime + buffer_time - self.buffer_time_low
|
|
# Under ran low buffer mark - flush lookahead queue
|
|
self._flush_lookahead(must_sync=True)
|
|
if print_time != self.print_time:
|
|
self.idle_flush_print_time = self.print_time
|
|
except:
|
|
logging.exception("Exception in flush_handler")
|
|
self.printer.invoke_shutdown("Exception in flush_handler")
|
|
return self.reactor.NEVER
|
|
# Movement commands
|
|
def get_position(self):
|
|
return list(self.commanded_pos)
|
|
def set_position(self, newpos, homing_axes=()):
|
|
self._flush_lookahead()
|
|
self.commanded_pos[:] = newpos
|
|
self.kin.set_position(newpos, homing_axes)
|
|
def move(self, newpos, speed):
|
|
speed = min(speed, self.max_velocity)
|
|
move = Move(self, self.commanded_pos, newpos, speed)
|
|
if not move.move_d:
|
|
return
|
|
if move.is_kinematic_move:
|
|
self.kin.check_move(move)
|
|
if move.axes_d[3]:
|
|
self.extruder.check_move(move)
|
|
self.commanded_pos[:] = move.end_pos
|
|
self.move_queue.add_move(move)
|
|
if self.print_time > self.need_check_stall:
|
|
self._check_stall()
|
|
def dwell(self, delay, check_stall=True):
|
|
self.get_last_move_time()
|
|
self.update_move_time(delay)
|
|
if check_stall:
|
|
self._check_stall()
|
|
def motor_off(self):
|
|
self.dwell(STALL_TIME)
|
|
last_move_time = self.get_last_move_time()
|
|
self.kin.motor_off(last_move_time)
|
|
for ext in kinematics.extruder.get_printer_extruders(self.printer):
|
|
ext.motor_off(last_move_time)
|
|
self.dwell(STALL_TIME)
|
|
logging.debug('; Max time of %f', last_move_time)
|
|
def wait_moves(self):
|
|
self._flush_lookahead()
|
|
if self.mcu.is_fileoutput():
|
|
return
|
|
eventtime = self.reactor.monotonic()
|
|
while (not self.sync_print_time
|
|
or self.print_time >= self.mcu.estimated_print_time(eventtime)):
|
|
eventtime = self.reactor.pause(eventtime + 0.100)
|
|
def set_extruder(self, extruder):
|
|
last_move_time = self.get_last_move_time()
|
|
self.extruder.set_active(last_move_time, False)
|
|
extrude_pos = extruder.set_active(last_move_time, True)
|
|
self.extruder = extruder
|
|
self.move_queue.set_extruder(extruder)
|
|
self.commanded_pos[3] = extrude_pos
|
|
def get_extruder(self):
|
|
return self.extruder
|
|
# Misc commands
|
|
def stats(self, eventtime):
|
|
for m in self.all_mcus:
|
|
m.check_active(self.print_time, eventtime)
|
|
buffer_time = self.print_time - self.mcu.estimated_print_time(eventtime)
|
|
is_active = buffer_time > -60. or not self.sync_print_time
|
|
return is_active, "print_time=%.3f buffer_time=%.3f print_stall=%d" % (
|
|
self.print_time, max(buffer_time, 0.), self.print_stall)
|
|
def get_status(self, eventtime):
|
|
print_time = self.print_time
|
|
estimated_print_time = self.mcu.estimated_print_time(eventtime)
|
|
last_print_start_time = self.last_print_start_time
|
|
buffer_time = print_time - estimated_print_time
|
|
if buffer_time > -1. or not self.sync_print_time:
|
|
status = "Printing"
|
|
else:
|
|
status = "Ready"
|
|
return { 'status': status, 'print_time': print_time,
|
|
'estimated_print_time': estimated_print_time,
|
|
'printing_time': print_time - last_print_start_time }
|
|
def printer_state(self, state):
|
|
if state == 'shutdown':
|
|
try:
|
|
self.move_queue.reset()
|
|
self.reset_print_time()
|
|
except:
|
|
logging.exception("Exception in toolhead shutdown")
|
|
def get_kinematics(self):
|
|
return self.kin
|
|
def get_max_velocity(self):
|
|
return self.max_velocity, self.max_accel
|
|
def get_max_axis_halt(self):
|
|
# Determine the maximum velocity a cartesian axis could halt
|
|
# at due to the junction_deviation setting. The 8.0 was
|
|
# determined experimentally.
|
|
return min(self.max_velocity,
|
|
math.sqrt(8. * self.junction_deviation * self.max_accel))
|
|
def _calc_junction_deviation(self):
|
|
scv2 = self.square_corner_velocity**2
|
|
self.junction_deviation = scv2 * (math.sqrt(2.) - 1.) / self.max_accel
|
|
cmd_SET_VELOCITY_LIMIT_help = "Set printer velocity limits"
|
|
def cmd_SET_VELOCITY_LIMIT(self, params):
|
|
print_time = self.get_last_move_time()
|
|
gcode = self.printer.lookup_object('gcode')
|
|
max_velocity = gcode.get_float(
|
|
'VELOCITY', params, self.max_velocity,
|
|
above=0., maxval=self.config_max_velocity)
|
|
max_accel = gcode.get_float(
|
|
'ACCEL', params, self.max_accel,
|
|
above=0., maxval=self.config_max_accel)
|
|
square_corner_velocity = gcode.get_float(
|
|
'SQUARE_CORNER_VELOCITY', params, self.square_corner_velocity,
|
|
minval=0., maxval=self.config_square_corner_velocity)
|
|
self.requested_accel_to_decel = gcode.get_float(
|
|
'ACCEL_TO_DECEL', params, self.requested_accel_to_decel, above=0.)
|
|
self.max_velocity = max_velocity
|
|
self.max_accel = max_accel
|
|
self.max_accel_to_decel = min(self.requested_accel_to_decel, max_accel)
|
|
self.square_corner_velocity = square_corner_velocity
|
|
self._calc_junction_deviation()
|
|
msg = ("max_velocity: %.6f\n"
|
|
"max_accel: %.6f\n"
|
|
"max_accel_to_decel: %.6f\n"
|
|
"square_corner_velocity: %.6f"% (
|
|
max_velocity, max_accel, self.requested_accel_to_decel,
|
|
square_corner_velocity))
|
|
self.printer.set_rollover_info("toolhead", "toolhead: %s" % (msg,))
|
|
gcode.respond_info(msg)
|
|
def cmd_M204(self, params):
|
|
gcode = self.printer.lookup_object('gcode')
|
|
if 'P' in params and 'T' in params and 'S' not in params:
|
|
# Use minimum of P and T for accel
|
|
accel = min(gcode.get_float('P', params, above=0.),
|
|
gcode.get_float('T', params, above=0.))
|
|
else:
|
|
# Use S for accel
|
|
accel = gcode.get_float('S', params, above=0.)
|
|
self.max_accel = min(accel, self.config_max_accel)
|
|
self._calc_junction_deviation()
|
|
|
|
def add_printer_objects(config):
|
|
config.get_printer().add_object('toolhead', ToolHead(config))
|
|
kinematics.extruder.add_printer_objects(config)
|