klipper-dgus/klippy/toolhead.py

# Code for coordinating events on the printer toolhead
#
# Copyright (C) 2016  Kevin O'Connor <kevin@koconnor.net>
#
# This file may be distributed under the terms of the GNU GPLv3 license.
import math, logging, time
import cartesian, delta

EXTRUDE_DIFF_IGNORE = 1.02

# Common suffixes: _d is distance (in mm), _v is velocity (in
#   mm/second), _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, accel):
        self.toolhead = toolhead
        self.start_pos = tuple(start_pos)
        self.end_pos = tuple(end_pos)
        self.accel = accel
        self.do_calc_junction = True
        self.axes_d = axes_d = [end_pos[i] - start_pos[i] for i in (0, 1, 2, 3)]
        if axes_d[2]:
            # Move with Z
            move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
            self.do_calc_junction = False
        else:
            move_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
            if not move_d:
                # Extrude only move
                move_d = abs(axes_d[3])
                if not move_d:
                    # No move
                    self.move_d = 0.
                    return
                self.do_calc_junction = False
        self.move_d = move_d
        self.extrude_r = axes_d[3] / move_d
        # Junction speeds are velocities squared.  The junction_delta
        # is the maximum amount of this squared-velocity that can
        # change in this move.
        self.junction_max = speed**2
        self.junction_delta = 2.0 * move_d * accel
        self.junction_start_max = 0.
    def limit_speed(self, speed, accel):
        self.junction_max = min(self.junction_max, speed**2)
        self.accel = min(self.accel, accel)
        self.junction_delta = 2.0 * self.move_d * self.accel
    def calc_junction(self, prev_move):
        if not self.do_calc_junction or not prev_move.do_calc_junction:
            return
        # Find max junction_start_velocity between two moves
        if (self.extrude_r > prev_move.extrude_r * EXTRUDE_DIFF_IGNORE
            or prev_move.extrude_r > self.extrude_r * EXTRUDE_DIFF_IGNORE):
            # Extrude ratio between moves is too different
            return
        self.extrude_r = prev_move.extrude_r
        # Find max velocity using approximated centripetal velocity as
        # described at:
        # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
        junction_cos_theta = -((self.axes_d[0] * prev_move.axes_d[0]
                                + self.axes_d[1] * prev_move.axes_d[1])
                               / (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)
        self.junction_start_max = min(
            R * self.accel, self.junction_max, prev_move.junction_max
            , prev_move.junction_start_max + prev_move.junction_delta)
    def process(self, junction_start, junction_cruise, junction_end
                , cornering_min, cornering_max):
        # Determine accel, cruise, and decel portions of the move distance
        inv_junction_delta = 1. / self.junction_delta
        accel_r = (junction_cruise-junction_start) * inv_junction_delta
        decel_r = (junction_cruise-junction_end) * inv_junction_delta
        cruise_r = 1. - accel_r - decel_r
        self.accel_r, self.cruise_r, self.decel_r = accel_r, cruise_r, decel_r
        # Determine move velocities
        start_v = math.sqrt(junction_start)
        cruise_v = math.sqrt(junction_cruise)
        end_v = math.sqrt(junction_end)
        self.start_v, self.cruise_v, self.end_v = start_v, cruise_v, end_v
        self.corner_min = math.sqrt(cornering_min)
        self.corner_max = math.sqrt(cornering_max)
        # Determine time spent in each portion of move (time is the
        # distance divided by average velocity)
        accel_t = accel_r * self.move_d / ((start_v + cruise_v) * 0.5)
        cruise_t = cruise_r * self.move_d / cruise_v
        decel_t = decel_r * self.move_d / ((end_v + cruise_v) * 0.5)
        self.accel_t, self.cruise_t, self.decel_t = accel_t, cruise_t, decel_t
        # Generate step times for the move
        next_move_time = self.toolhead.get_next_move_time()
        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(accel_t + cruise_t + decel_t)

# 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.queue = []
        self.junction_flush = 0.
    def reset(self):
        del self.queue[:]
    def flush(self, lazy=False):
        flush_count = len(self.queue)
        move_info = [None] * flush_count
        # Traverse queue from last to first move and determine maximum
        # junction speed assuming the robot comes to a complete stop
        # after the last move.
        next_junction_end = cornering_min = cornering_max = 0.
        for i in range(flush_count-1, -1, -1):
            move = self.queue[i]
            reachable_start = next_junction_end + move.junction_delta
            junction_start = min(move.junction_start_max, reachable_start)
            junction_cruise = min((junction_start + reachable_start) * .5
                                  , move.junction_max)
            move_info[i] = (junction_start, junction_cruise, next_junction_end
                            , cornering_min, cornering_max)
            if reachable_start > junction_start:
                cornering_min = junction_start
                if junction_start + move.junction_delta > next_junction_end:
                    cornering_max = junction_cruise
                    if lazy:
                        flush_count = i
                        lazy = False
            next_junction_end = junction_start
        if lazy:
            flush_count = 0
        # Generate step times for all moves ready to be flushed
        for i in range(flush_count):
            self.queue[i].process(*move_info[i])
        # Remove processed moves from the queue
        del self.queue[:flush_count]
        if self.queue:
            self.junction_flush = 2. * self.queue[-1].junction_max
    def add_move(self, move):
        self.queue.append(move)
        if len(self.queue) == 1:
            self.junction_flush = 2. * move.junction_max
            return
        move.calc_junction(self.queue[-2])
        self.junction_flush -= move.junction_delta
        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, printer, config):
        self.printer = printer
        self.reactor = printer.reactor
        self.extruder = printer.objects.get('extruder')
        kintypes = {'cartesian': cartesian.CartKinematics,
                    'delta': delta.DeltaKinematics}
        self.kin = config.getchoice('kinematics', kintypes)(printer, config)
        self.max_speed = config.getfloat('max_velocity')
        self.max_accel = config.getfloat('max_accel')
        self.junction_deviation = config.getfloat('junction_deviation', 0.02)
        self.move_queue = MoveQueue()
        self.commanded_pos = [0., 0., 0., 0.]
        # Print time tracking
        self.buffer_time_high = config.getfloat('buffer_time_high', 5.000)
        self.buffer_time_low = config.getfloat('buffer_time_low', 0.150)
        self.move_flush_time = config.getfloat('move_flush_time', 0.050)
        self.motor_off_delay = config.getfloat('motor_off_time', 60.000)
        self.print_time = 0.
        self.print_time_stall = 0
        self.motor_off_time = self.reactor.NEVER
        self.flush_timer = self.reactor.register_timer(self.flush_handler)
    def build_config(self):
        self.kin.set_max_jerk(0.005 * self.max_accel, self.max_accel) # XXX
        self.kin.build_config()
    # Print time tracking
    def update_move_time(self, movetime):
        self.print_time += movetime
        flush_to_time = self.print_time - self.move_flush_time
        self.printer.mcu.flush_moves(flush_to_time)
    def get_next_move_time(self):
        if not self.print_time:
            self.print_time = self.buffer_time_low + STALL_TIME
            curtime = time.time()
            self.printer.mcu.set_print_start_time(curtime)
            self.reactor.update_timer(self.flush_timer, self.reactor.NOW)
        return self.print_time
    def get_last_move_time(self):
        self.move_queue.flush()
        return self.get_next_move_time()
    def reset_motor_off_time(self, eventtime):
        self.motor_off_time = eventtime + self.motor_off_delay
    def reset_print_time(self):
        self.move_queue.flush()
        self.printer.mcu.flush_moves(self.print_time)
        self.print_time = 0.
        self.reset_motor_off_time(time.time())
        self.reactor.update_timer(self.flush_timer, self.motor_off_time)
    def check_busy(self, eventtime):
        if not self.print_time:
            # XXX - find better way to flush initial move_queue items
            if self.move_queue.queue:
                self.reactor.update_timer(self.flush_timer, eventtime + 0.100)
            return False
        buffer_time = self.printer.mcu.get_print_buffer_time(
            eventtime, self.print_time)
        return buffer_time > self.buffer_time_high
    def flush_handler(self, eventtime):
        try:
            if not self.print_time:
                self.move_queue.flush()
                if not self.print_time:
                    if eventtime >= self.motor_off_time:
                        self.motor_off()
                        self.reset_print_time()
                        self.motor_off_time = self.reactor.NEVER
                    return self.motor_off_time
            print_time = self.print_time
            buffer_time = self.printer.mcu.get_print_buffer_time(
                eventtime, print_time)
            if buffer_time > self.buffer_time_low:
                return eventtime + buffer_time - self.buffer_time_low
            self.move_queue.flush()
            if print_time != self.print_time:
                self.print_time_stall += 1
                self.dwell(self.buffer_time_low + STALL_TIME)
                return self.reactor.NOW
            self.reset_print_time()
            return self.motor_off_time
        except:
            logging.exception("Exception in flush_handler")
            self.force_shutdown()
    def stats(self, eventtime):
        buffer_time = 0.
        if self.print_time:
            buffer_time = self.printer.mcu.get_print_buffer_time(
                eventtime, self.print_time)
        return "print_time=%.3f buffer_time=%.3f print_time_stall=%d" % (
            self.print_time, buffer_time, self.print_time_stall)
    # Movement commands
    def get_position(self):
        return list(self.commanded_pos)
    def set_position(self, newpos):
        self.move_queue.flush()
        self.commanded_pos[:] = newpos
        self.kin.set_position(newpos)
    def move(self, newpos, speed):
        speed = min(speed, self.max_speed)
        move = Move(self, self.commanded_pos, newpos, speed, self.max_accel)
        if not move.move_d:
            return
        self.kin.check_move(move)
        if move.axes_d[3]:
            self.extruder.check_move(move)
        self.commanded_pos[:] = newpos
        self.move_queue.add_move(move)
    def home(self, homing_state):
        self.kin.home(homing_state)
    def dwell(self, delay):
        self.get_last_move_time()
        self.update_move_time(delay)
    def motor_off(self):
        self.dwell(STALL_TIME)
        last_move_time = self.get_last_move_time()
        self.kin.motor_off(last_move_time)
        self.extruder.motor_off(last_move_time)
        self.dwell(STALL_TIME)
        logging.debug('; Max time of %f' % (last_move_time,))
    def query_endstops(self, query_state):
        return self.kin.query_endstops(query_state)
    def force_shutdown(self):
        self.printer.mcu.force_shutdown()
        self.move_queue.reset()