Initial commit of source code.

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

1 files changed, 252 insertions, 0 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
new file mode 100644
index 00000000..40840077
--- /dev/null
+++ b/klippy/cartesian.py
@@ -0,0 +1,252 @@
+# Code for handling cartesian (standard x, y, z planes) moves
+#
+# 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)
+
+StepList = (0, 1, 2, 3)
+
+class Move:
+    def __init__(self, kin, relsteps, speed):
+        self.kin = kin
+        self.relsteps = relsteps
+        self.junction_max = self.junction_start_max = self.junction_delta = 0.
+        # Calculate requested distance to travel (in mm)
+        steppers = self.kin.steppers
+        absrelsteps = [abs(relsteps[i]) for i in StepList]
+        stepper_d = [absrelsteps[i] * steppers[i].step_dist
+                     for i in StepList]
+        self.move_d = math.sqrt(sum([d*d for d in stepper_d[:3]]))
+        if not self.move_d:
+            self.move_d = stepper_d[3]
+            if not self.move_d:
+                return
+        # Limit velocity to max for each stepper
+        velocity_factor = min([steppers[i].max_step_velocity / absrelsteps[i]
+                               for i in StepList if absrelsteps[i]])
+        move_v = min(speed, velocity_factor * self.move_d)
+        self.junction_max = move_v**2
+        # Find max acceleration factor
+        accel_factor = min([steppers[i].max_step_accel / absrelsteps[i]
+                            for i in StepList if absrelsteps[i]])
+        accel = min(self.kin.max_accel, accel_factor * self.move_d)
+        self.junction_delta = 2.0 * self.move_d * accel
+    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 or self.relsteps[2] or prev_move.relsteps[2]:
+            return
+        steppers = self.kin.steppers
+        junction_cos_theta = -sum([
+            self.relsteps[i] * prev_move.relsteps[i] * steppers[i].step_dist**2
+            for i in range(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.kin.junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)
+        accel = self.junction_delta / (2.0 * self.move_d)
+        self.junction_start_max = min(
+            accel * R, 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
+        # 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)
+        inv_cruise_v = 1. / cruise_v
+        inv_accel = 2.0 * self.move_d * inv_junction_delta
+        accel_t = 2.0 * self.move_d * accel_r / (start_v+cruise_v)
+        cruise_t = self.move_d * cruise_r * inv_cruise_v
+        decel_t = 2.0 * self.move_d * decel_r / (end_v+cruise_v)
+
+        #logging.debug("Move: %s v=%.2f/%.2f/%.2f mt=%.3f st=%.3f %.3f %.3f" % (
+        #    self.relsteps, start_v, cruise_v, end_v, move_t
+        #    , next_move_time, accel_r, cruise_r))
+
+        # Calculate step times for the move
+        next_move_time = self.kin.get_next_move_time()
+        for i in StepList:
+            steps = self.relsteps[i]
+            if not steps:
+                continue
+            sdir = 0
+            if steps < 0:
+                sdir = 1
+                steps = -steps
+            clock_offset, clock_freq, so = self.kin.steppers[i].prep_move(
+                sdir, next_move_time)
+
+            step_dist = self.move_d / steps
+            step_offset = 0.5
+
+            # Acceleration steps
+            #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
+            accel_clock_offset = start_v * inv_accel * clock_freq
+            accel_sqrt_offset = accel_clock_offset**2
+            accel_multiplier = 2.0 * step_dist * inv_accel * clock_freq**2
+            accel_steps = accel_r * steps
+            step_offset = so.step_sqrt(
+                accel_steps, step_offset, clock_offset - accel_clock_offset
+                , accel_sqrt_offset, accel_multiplier)
+            clock_offset += accel_t * clock_freq
+            # Cruising steps
+            #t = pos/cruise_v
+            cruise_multiplier = step_dist * inv_cruise_v * clock_freq
+            cruise_steps = cruise_r * steps
+            step_offset = so.step_factor(
+                cruise_steps, step_offset, clock_offset, cruise_multiplier)
+            clock_offset += cruise_t * clock_freq
+            # Deceleration steps
+            #t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
+            decel_clock_offset = cruise_v * inv_accel * clock_freq
+            decel_sqrt_offset = decel_clock_offset**2
+            decel_steps = decel_r * steps
+            so.step_sqrt(
+                decel_steps, step_offset, clock_offset + decel_clock_offset
+                , decel_sqrt_offset, -accel_multiplier)
+        self.kin.update_move_time(accel_t + cruise_t + decel_t)
+
+STALL_TIME = 0.100
+
+class CartKinematics:
+    def __init__(self, printer, config):
+        self.printer = printer
+        self.reactor = printer.reactor
+        steppers = ['stepper_x', 'stepper_y', 'stepper_z', 'stepper_e']
+        self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
+                         for n in steppers]
+        self.max_accel = min(s.max_step_accel*s.step_dist
+                             for s in self.steppers[:2]) # XXX
+        dummy_move = Move(self, [0]*len(self.steppers), 0.)
+        dummy_move.junction_max = 0.
+        self.junction_deviation = config.getfloat('junction_deviation', 0.02)
+        self.move_queue = lookahead.MoveQueue(dummy_move)
+        self.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):
+        for stepper in self.steppers:
+            stepper.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 [self.pos[i] * self.steppers[i].step_dist
+                for i in StepList]
+    def set_position(self, newpos):
+        self.pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
+                    for i in StepList]
+    def move(self, newpos, speed, sloppy=False):
+        # Round to closest step position
+        newpos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
+                  for i in StepList]
+        relsteps = [newpos[i] - self.pos[i] for i in StepList]
+        self.pos = newpos
+        if relsteps == [0]*len(newpos):
+            # no move
+            return
+        #logging.debug("; dist %s @ %d\n" % (
+        #    [newpos[i]*self.steppers[i].step_dist for i in StepList], speed))
+        # Create move and queue it
+        move = Move(self, relsteps, speed)
+        move.calc_junction(self.move_queue.prev_move())
+        self.move_queue.add_move(move)
+    def home(self, axis):
+        # Each axis is homed independently and in order
+        homing_state = homing.Homing(self, self.steppers)
+        for a in axis:
+            homing_state.plan_home(a)
+        return 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()
+        for stepper in self.steppers:
+            stepper.motor_enable(last_move_time, 0)
+        self.dwell(STALL_TIME)
+        logging.debug('; Max time of %f' % (last_move_time,))