b3e8b430e5
Perform all acceleration calculations and lookahead checks in millimeters using the cartesian coordinate system of the request. The conversion to step coordinates is now done at the time of the step timing creation. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
266 lines
12 KiB
Python
266 lines
12 KiB
Python
# 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, pos, move_d, axes_d, speed, accel):
|
|
self.kin = kin
|
|
self.pos = tuple(pos)
|
|
self.axes_d = axes_d
|
|
self.move_d = move_d
|
|
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.kin.junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)
|
|
self.junction_start_max = min(
|
|
R * self.kin.max_xy_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
|
|
# 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:
|
|
new_step_pos = int(self.pos[i]*self.kin.steppers[i].inv_step_dist
|
|
+ 0.5)
|
|
steps = new_step_pos - self.kin.stepper_pos[i]
|
|
if not steps:
|
|
continue
|
|
self.kin.stepper_pos[i] = new_step_pos
|
|
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_xy_speed = min(s.max_step_velocity*s.step_dist
|
|
for s in self.steppers[:2])
|
|
self.max_xy_accel = min(s.max_step_accel*s.step_dist
|
|
for s in self.steppers[:2])
|
|
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.]
|
|
self.stepper_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 list(self.commanded_pos)
|
|
def set_position(self, newpos):
|
|
self.move_queue.flush()
|
|
self.commanded_pos[:] = newpos
|
|
self.stepper_pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
|
|
for i in StepList]
|
|
def _move_with_z(self, newpos, axes_d, speed):
|
|
self.move_queue.flush()
|
|
# Limit velocity to max for each stepper
|
|
move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
|
|
velocity_factor = min(
|
|
[self.steppers[i].max_step_velocity
|
|
* self.steppers[i].step_dist / abs(axes_d[i])
|
|
for i in StepList if axes_d[i]])
|
|
speed = min(speed, self.max_xy_speed, velocity_factor * move_d)
|
|
# Find max acceleration factor
|
|
accel_factor = min(
|
|
[self.steppers[i].max_step_accel
|
|
* self.steppers[i].step_dist / abs(axes_d[i])
|
|
for i in StepList if axes_d[i]])
|
|
accel = min(self.max_xy_accel, accel_factor * move_d)
|
|
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()
|
|
s = self.steppers[3]
|
|
speed = min(speed, self.max_xy_speed, s.max_step_velocity * s.step_dist)
|
|
accel = min(self.max_xy_accel, s.max_step_accel * s.step_dist)
|
|
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):
|
|
# 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,))
|