diff options
| -rw-r--r-- | klippy/cartesian.py | 198 | ||||
| -rw-r--r-- | klippy/klippy.py | 4 | ||||
| -rw-r--r-- | klippy/toolhead.py | 200 |
3 files changed, 205 insertions, 197 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,)) diff --git a/klippy/klippy.py b/klippy/klippy.py index a8d50c5e..421a6099 100644 --- a/klippy/klippy.py +++ b/klippy/klippy.py @@ -5,7 +5,7 @@ # # This file may be distributed under the terms of the GNU GPLv3 license. import sys, optparse, ConfigParser, logging, time, threading -import gcode, cartesian, util, mcu, fan, heater, reactor +import gcode, toolhead, util, mcu, fan, heater, reactor class ConfigWrapper: def __init__(self, printer, section): @@ -58,7 +58,7 @@ class Printer: if self.fileconfig.has_section('heater_bed'): self.objects['heater_bed'] = heater.PrinterHeater( self, ConfigWrapper(self, 'heater_bed')) - self.objects['toolhead'] = cartesian.ToolHead( + self.objects['toolhead'] = toolhead.ToolHead( self, self._pconfig) def stats(self, eventtime): diff --git a/klippy/toolhead.py b/klippy/toolhead.py new file mode 100644 index 00000000..b89ef2f3 --- /dev/null +++ b/klippy/toolhead.py @@ -0,0 +1,200 @@ +# 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 lookahead, cartesian + +# 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 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 = cartesian.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 (0, 1, 2, 3)] + 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,)) |