Mirror
/
klipper-dgus
mirror of https://github.com/Desuuuu/klipper.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

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>
This commit is contained in:
Kevin O'Connor 2016-07-07 15:52:44 -04:00
parent 861f5a5387
commit e0a9a1b800
3 changed files with 205 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

198
klippy/cartesian.py
View File

@ -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,))

4
klippy/klippy.py
View File

@ -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):

200
klippy/toolhead.py Normal file
View File

@ -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,))