toolhead: Allow kinematics class to verify the move prior to queuing it

Introduce a check_move() method in the extruder and cartesian
kinematic classes.  This allows the lower level classes to verify the
contents of the move prior to queing that move.

The speed and acceleration handling for special Z and extrude only
moves are also moved from the generic toolhead class to the low-level
classes.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2016-09-30 14:47:45 -04:00
parent e9505697fb
commit 275b386856
3 changed files with 47 additions and 41 deletions

View File

@ -22,17 +22,10 @@ class CartKinematics:
def set_position(self, newpos): def set_position(self, newpos):
self.stepper_pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5) self.stepper_pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5)
for i in StepList] for i in StepList]
def get_max_xy_speed(self): def get_max_speed(self):
max_xy_speed = min(s.max_velocity for s in self.steppers[:2]) max_xy_speed = min(s.max_velocity for s in self.steppers[:2])
max_xy_accel = min(s.max_accel for s in self.steppers[:2]) max_xy_accel = min(s.max_accel for s in self.steppers[:2])
return max_xy_speed, max_xy_accel return max_xy_speed, max_xy_accel
def get_max_speed(self, axes_d, move_d):
# Calculate max speed and accel for a given move
velocity_factor = min([self.steppers[i].max_velocity / abs(axes_d[i])
for i in StepList if axes_d[i]])
accel_factor = min([self.steppers[i].max_accel / abs(axes_d[i])
for i in StepList if axes_d[i]])
return velocity_factor * move_d, accel_factor * move_d
def get_homed_position(self): def get_homed_position(self):
return [s.get_homed_position() for s in self.steppers] return [s.get_homed_position() for s in self.steppers]
def home(self, toolhead, axes): def home(self, toolhead, axes):
@ -69,6 +62,18 @@ class CartKinematics:
stepper.motor_enable(move_time, 0) stepper.motor_enable(move_time, 0)
def query_endstops(self, move_time): def query_endstops(self, move_time):
return homing.QueryEndstops(["x", "y", "z"], self.steppers) return homing.QueryEndstops(["x", "y", "z"], self.steppers)
def check_move(self, move):
if not move.axes_d[2]:
# Normal XY move - use defaults
return
# Move with Z - update velocity and accel for slower Z axis
axes_d = move.axes_d
move_d = move.move_d
velocity_factor = min([self.steppers[i].max_velocity / abs(axes_d[i])
for i in StepList if axes_d[i]])
accel_factor = min([self.steppers[i].max_accel / abs(axes_d[i])
for i in StepList if axes_d[i]])
move.limit_speed(velocity_factor * move_d, accel_factor * move_d)
def move(self, move_time, move): def move(self, move_time, move):
inv_accel = 1. / move.accel inv_accel = 1. / move.accel
inv_cruise_v = 1. / move.cruise_v inv_cruise_v = 1. / move.cruise_v

View File

@ -18,10 +18,14 @@ class PrinterExtruder:
self.heater.build_config() self.heater.build_config()
self.stepper.set_max_jerk(9999999.9) self.stepper.set_max_jerk(9999999.9)
self.stepper.build_config() self.stepper.build_config()
def get_max_speed(self):
return self.stepper.max_velocity, self.stepper.max_accel
def motor_off(self, move_time): def motor_off(self, move_time):
self.stepper.motor_enable(move_time, 0) self.stepper.motor_enable(move_time, 0)
def check_move(self, move):
if (not move.do_calc_junction
and not move.axes_d[0] and not move.axes_d[1]
and not move.axes_d[2]):
# Extrude only move - limit accel and velocity
move.limit_speed(self.stepper.max_velocity, self.stepper.max_accel)
def move(self, move_time, move): def move(self, move_time, move):
move_d = move.move_d move_d = move.move_d
inv_accel = 1. / move.accel inv_accel = 1. / move.accel

View File

@ -14,12 +14,23 @@ EXTRUDE_DIFF_IGNORE = 1.02
# Class to track each move request # Class to track each move request
class Move: class Move:
def __init__(self, toolhead, pos, move_d, axes_d, speed, accel): def __init__(self, toolhead, pos, axes_d, speed, accel):
self.toolhead = toolhead self.toolhead = toolhead
self.pos = tuple(pos) self.pos = tuple(pos)
self.move_d = move_d
self.axes_d = axes_d self.axes_d = axes_d
self.accel = accel self.accel = accel
self.do_calc_junction = True
if axes_d[2]:
# Move with Z
move_d = math.sqrt(sum([d*d for d in axes_d[:3]]))
self.do_calc_junction = False
else:
move_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2)
if not move_d:
# Extrude only move
move_d = abs(axes_d[3])
self.do_calc_junction = False
self.move_d = move_d
self.extrude_r = axes_d[3] / move_d self.extrude_r = axes_d[3] / move_d
# Junction speeds are velocities squared. The junction_delta # Junction speeds are velocities squared. The junction_delta
# is the maximum amount of this squared-velocity that can # is the maximum amount of this squared-velocity that can
@ -27,7 +38,13 @@ class Move:
self.junction_max = speed**2 self.junction_max = speed**2
self.junction_delta = 2.0 * move_d * accel self.junction_delta = 2.0 * move_d * accel
self.junction_start_max = 0. self.junction_start_max = 0.
def limit_speed(self, speed, accel):
self.junction_max = min(self.junction_max, speed**2)
self.accel = min(self.accel, accel)
self.junction_delta = 2.0 * self.move_d * self.accel
def calc_junction(self, prev_move): def calc_junction(self, prev_move):
if not self.do_calc_junction or not prev_move.do_calc_junction:
return
# Find max junction_start_velocity between two moves # Find max junction_start_velocity between two moves
if (self.extrude_r > prev_move.extrude_r * EXTRUDE_DIFF_IGNORE if (self.extrude_r > prev_move.extrude_r * EXTRUDE_DIFF_IGNORE
or prev_move.extrude_r > self.extrude_r * EXTRUDE_DIFF_IGNORE): or prev_move.extrude_r > self.extrude_r * EXTRUDE_DIFF_IGNORE):
@ -141,7 +158,7 @@ class ToolHead:
self.reactor = printer.reactor self.reactor = printer.reactor
self.extruder = printer.objects.get('extruder') self.extruder = printer.objects.get('extruder')
self.kin = cartesian.CartKinematics(printer, config) self.kin = cartesian.CartKinematics(printer, config)
self.max_xy_speed, self.max_xy_accel = self.kin.get_max_xy_speed() self.max_speed, self.max_accel = self.kin.get_max_speed()
self.junction_deviation = config.getfloat('junction_deviation', 0.02) self.junction_deviation = config.getfloat('junction_deviation', 0.02)
self.move_queue = MoveQueue() self.move_queue = MoveQueue()
self.commanded_pos = [0., 0., 0., 0.] self.commanded_pos = [0., 0., 0., 0.]
@ -227,38 +244,18 @@ class ToolHead:
self.move_queue.flush() self.move_queue.flush()
self.commanded_pos[:] = newpos self.commanded_pos[:] = newpos
self.kin.set_position(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.extruder.get_max_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): def move(self, newpos, speed, sloppy=False):
axes_d = [newpos[i] - self.commanded_pos[i] axes_d = [newpos[i] - self.commanded_pos[i]
for i in (0, 1, 2, 3)] for i in (0, 1, 2, 3)]
self.commanded_pos[:] = newpos if axes_d == [0., 0., 0., 0.]:
if axes_d[2]: # No move
self._move_with_z(newpos, axes_d, speed)
return return
move_d = math.sqrt(axes_d[0]**2 + axes_d[1]**2) speed = min(speed, self.max_speed)
if not move_d: move = Move(self, newpos, axes_d, speed, self.max_accel)
self.kin.check_move(move)
if axes_d[3]: if axes_d[3]:
self._move_only_e(newpos, axes_d, speed) self.extruder.check_move(move)
return self.commanded_pos[:] = newpos
# 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)
self.move_queue.add_move(move) self.move_queue.add_move(move)
def home(self, axes): def home(self, axes):
homing = self.kin.home(self, axes) homing = self.kin.home(self, axes)