toolhead: Split toolhead code from cartesian.py to new file toolhead.py

Separate out the toolhead logic to its own python file.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

1 files changed, 3 insertions, 195 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 487e47b7..8be83a71 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -1,14 +1,10 @@
-# Code for handling cartesian (standard x, y, z planes) moves
+# Code for handling the kinematics of cartesian robots
 #
 # 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 lookahead, stepper, homing
-
-# 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)
+import logging
+import stepper, homing
 
 StepList = (0, 1, 2, 3)
 
@@ -97,191 +93,3 @@ class CartKinematics:
             so.step_sqrt(
                 decel_steps, step_offset, clock_offset + decel_clock_offset
                 , decel_sqrt_offset, -accel_multiplier)
-
-class Move:
-    def __init__(self, toolhead, pos, move_d, axes_d, speed, accel):
-        self.toolhead = toolhead
-        self.pos = tuple(pos)
-        self.move_d = move_d
-        self.axes_d = axes_d
-        self.accel = accel
-        self.junction_max = speed**2
-        self.junction_delta = 2.0 * move_d * accel
-        self.junction_start_max = 0.
-    def calc_junction(self, prev_move):
-        # Find max start junction velocity using approximated
-        # centripetal velocity as described at:
-        # https://onehossshay.wordpress.com/2011/09/24/improving_grbl_cornering_algorithm/
-        if not prev_move.move_d:
-            return
-        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)
-    def process(self, junction_start, junction_end):
-        # Determine accel, cruise, and decel portions of the move
-        junction_cruise = self.junction_max
-        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
-        if cruise_r < 0.:
-            accel_r += 0.5 * cruise_r
-            decel_r = 1.0 - accel_r
-            cruise_r = 0.
-            junction_cruise = junction_start + accel_r*self.junction_delta
-        self.accel_r, self.cruise_r, self.decel_r = accel_r, cruise_r, decel_r
-        # Determine the move velocities and time spent in each portion
-        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
-        accel_t = 2.0 * self.move_d * accel_r / (start_v + cruise_v)
-        cruise_t = self.move_d * cruise_r / cruise_v
-        decel_t = 2.0 * self.move_d * decel_r / (end_v + cruise_v)
-        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)
-        self.toolhead.update_move_time(accel_t + cruise_t + decel_t)
-
-STALL_TIME = 0.100
-
-class ToolHead:
-    def __init__(self, printer, config):
-        self.printer = printer
-        self.reactor = printer.reactor
-        self.kin = CartKinematics(printer, config)
-        self.max_xy_speed, self.max_xy_accel = self.kin.get_max_xy_speed()
-        self.junction_deviation = config.getfloat('junction_deviation', 0.02)
-        dummy_move = Move(self, [0.]*4, 0., [0.]*4, 0., 0.)
-        self.move_queue = lookahead.MoveQueue(dummy_move)
-        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.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):
-        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
-    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_with_z(self, newpos, axes_d, speed):
-        self.move_queue.flush()
-        move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
-        # Limit velocity and accel to max for each stepper
-        kin_speed, kin_accel = self.kin.get_max_speed(axes_d, move_d)
-        speed = min(speed, self.max_xy_speed, kin_speed)
-        accel = min(self.max_xy_accel, kin_accel)
-        # Generate and execute move
-        move = Move(self, newpos, move_d, axes_d, speed, accel)
-        move.process(0., 0.)
-    def _move_only_e(self, newpos, axes_d, speed):
-        self.move_queue.flush()
-        kin_speed, kin_accel = self.kin.get_max_e_speed()
-        speed = min(speed, self.max_xy_speed, kin_speed)
-        accel = min(self.max_xy_accel, kin_accel)
-        move = Move(self, newpos, abs(axes_d[3]), axes_d, speed, accel)
-        move.process(0., 0.)
-    def move(self, newpos, speed, sloppy=False):
-        axes_d = [newpos[i] - self.commanded_pos[i] for i in StepList]
-        self.commanded_pos[:] = newpos
-        if axes_d[2]:
-            self._move_with_z(newpos, axes_d, speed)
-            return
-        move_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
-        if not move_d:
-            if axes_d[3]:
-                self._move_only_e(newpos, axes_d, speed)
-            return
-        # Common xy move - create move and queue it
-        speed = min(speed, self.max_xy_speed)
-        move = Move(self, newpos, move_d, axes_d, speed, self.max_xy_accel)
-        move.calc_junction(self.move_queue.prev_move())
-        self.move_queue.add_move(move)
-    def home(self, axis):
-        return self.kin.home(self, axis)
-    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.dwell(STALL_TIME)
-        logging.debug('; Max time of %f' % (last_move_time,))