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:
parent
e9505697fb
commit
275b386856
|
@ -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
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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)
|
||||||
|
|
Loading…
Reference in New Issue