idex_modes: Native input shaping support with dual carriages
Signed-off-by: Dmitry Butyugin <dmbutyugin@google.com>
This commit is contained in:
parent
98ed0a8168
commit
345934bd68
|
@ -61,7 +61,6 @@ class AxisInputShaper:
|
||||||
self.params.update(gcmd)
|
self.params.update(gcmd)
|
||||||
old_n, old_A, old_T = self.n, self.A, self.T
|
old_n, old_A, old_T = self.n, self.A, self.T
|
||||||
self.n, self.A, self.T = self.params.get_shaper()
|
self.n, self.A, self.T = self.params.get_shaper()
|
||||||
return (old_n, old_A, old_T) != (self.n, self.A, self.T)
|
|
||||||
def set_shaper_kinematics(self, sk):
|
def set_shaper_kinematics(self, sk):
|
||||||
ffi_main, ffi_lib = chelper.get_ffi()
|
ffi_main, ffi_lib = chelper.get_ffi()
|
||||||
success = ffi_lib.input_shaper_set_shaper_params(
|
success = ffi_lib.input_shaper_set_shaper_params(
|
||||||
|
@ -98,7 +97,7 @@ class InputShaper:
|
||||||
self.toolhead = None
|
self.toolhead = None
|
||||||
self.shapers = [AxisInputShaper('x', config),
|
self.shapers = [AxisInputShaper('x', config),
|
||||||
AxisInputShaper('y', config)]
|
AxisInputShaper('y', config)]
|
||||||
self.stepper_kinematics = []
|
self.input_shaper_stepper_kinematics = []
|
||||||
self.orig_stepper_kinematics = []
|
self.orig_stepper_kinematics = []
|
||||||
# Register gcode commands
|
# Register gcode commands
|
||||||
gcode = self.printer.lookup_object('gcode')
|
gcode = self.printer.lookup_object('gcode')
|
||||||
|
@ -109,38 +108,50 @@ class InputShaper:
|
||||||
return self.shapers
|
return self.shapers
|
||||||
def connect(self):
|
def connect(self):
|
||||||
self.toolhead = self.printer.lookup_object("toolhead")
|
self.toolhead = self.printer.lookup_object("toolhead")
|
||||||
kin = self.toolhead.get_kinematics()
|
|
||||||
# Lookup stepper kinematics
|
|
||||||
ffi_main, ffi_lib = chelper.get_ffi()
|
|
||||||
steppers = kin.get_steppers()
|
|
||||||
for s in steppers:
|
|
||||||
sk = ffi_main.gc(ffi_lib.input_shaper_alloc(), ffi_lib.free)
|
|
||||||
orig_sk = s.set_stepper_kinematics(sk)
|
|
||||||
res = ffi_lib.input_shaper_set_sk(sk, orig_sk)
|
|
||||||
if res < 0:
|
|
||||||
s.set_stepper_kinematics(orig_sk)
|
|
||||||
continue
|
|
||||||
self.stepper_kinematics.append(sk)
|
|
||||||
self.orig_stepper_kinematics.append(orig_sk)
|
|
||||||
# Configure initial values
|
# Configure initial values
|
||||||
self.old_delay = 0.
|
self.old_delay = 0.
|
||||||
self._update_input_shaping(error=self.printer.config_error)
|
self._update_input_shaping(error=self.printer.config_error)
|
||||||
|
def _get_input_shaper_stepper_kinematics(self, stepper):
|
||||||
|
# Lookup stepper kinematics
|
||||||
|
sk = stepper.get_stepper_kinematics()
|
||||||
|
if sk in self.orig_stepper_kinematics:
|
||||||
|
# Already processed this stepper kinematics unsuccessfully
|
||||||
|
return None
|
||||||
|
if sk in self.input_shaper_stepper_kinematics:
|
||||||
|
return sk
|
||||||
|
self.orig_stepper_kinematics.append(sk)
|
||||||
|
ffi_main, ffi_lib = chelper.get_ffi()
|
||||||
|
is_sk = ffi_main.gc(ffi_lib.input_shaper_alloc(), ffi_lib.free)
|
||||||
|
stepper.set_stepper_kinematics(is_sk)
|
||||||
|
res = ffi_lib.input_shaper_set_sk(is_sk, sk)
|
||||||
|
if res < 0:
|
||||||
|
stepper.set_stepper_kinematics(sk)
|
||||||
|
return None
|
||||||
|
self.input_shaper_stepper_kinematics.append(is_sk)
|
||||||
|
return is_sk
|
||||||
def _update_input_shaping(self, error=None):
|
def _update_input_shaping(self, error=None):
|
||||||
self.toolhead.flush_step_generation()
|
self.toolhead.flush_step_generation()
|
||||||
new_delay = max([s.get_step_generation_window() for s in self.shapers])
|
new_delay = max([s.get_step_generation_window() for s in self.shapers])
|
||||||
self.toolhead.note_step_generation_scan_time(new_delay,
|
self.toolhead.note_step_generation_scan_time(new_delay,
|
||||||
old_delay=self.old_delay)
|
old_delay=self.old_delay)
|
||||||
failed = []
|
self.old_delay = new_delay
|
||||||
for sk in self.stepper_kinematics:
|
kin = self.toolhead.get_kinematics()
|
||||||
for shaper in self.shapers:
|
failed_shapers = []
|
||||||
if shaper in failed:
|
for s in kin.get_steppers():
|
||||||
|
if s.get_trapq() is None:
|
||||||
continue
|
continue
|
||||||
if not shaper.set_shaper_kinematics(sk):
|
is_sk = self._get_input_shaper_stepper_kinematics(s)
|
||||||
failed.append(shaper)
|
if is_sk is None:
|
||||||
if failed:
|
continue
|
||||||
|
for shaper in self.shapers:
|
||||||
|
if shaper in failed_shapers:
|
||||||
|
continue
|
||||||
|
if not shaper.set_shaper_kinematics(is_sk):
|
||||||
|
failed_shapers.append(shaper)
|
||||||
|
if failed_shapers:
|
||||||
error = error or self.printer.command_error
|
error = error or self.printer.command_error
|
||||||
raise error("Failed to configure shaper(s) %s with given parameters"
|
raise error("Failed to configure shaper(s) %s with given parameters"
|
||||||
% (', '.join([s.get_name() for s in failed])))
|
% (', '.join([s.get_name() for s in failed_shapers])))
|
||||||
def disable_shaping(self):
|
def disable_shaping(self):
|
||||||
for shaper in self.shapers:
|
for shaper in self.shapers:
|
||||||
shaper.disable_shaping()
|
shaper.disable_shaping()
|
||||||
|
@ -151,10 +162,9 @@ class InputShaper:
|
||||||
self._update_input_shaping()
|
self._update_input_shaping()
|
||||||
cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper"
|
cmd_SET_INPUT_SHAPER_help = "Set cartesian parameters for input shaper"
|
||||||
def cmd_SET_INPUT_SHAPER(self, gcmd):
|
def cmd_SET_INPUT_SHAPER(self, gcmd):
|
||||||
updated = False
|
if gcmd.get_command_parameters():
|
||||||
for shaper in self.shapers:
|
for shaper in self.shapers:
|
||||||
updated |= shaper.update(gcmd)
|
shaper.update(gcmd)
|
||||||
if updated:
|
|
||||||
self._update_input_shaping()
|
self._update_input_shaping()
|
||||||
for shaper in self.shapers:
|
for shaper in self.shapers:
|
||||||
shaper.report(gcmd)
|
shaper.report(gcmd)
|
||||||
|
|
|
@ -4,6 +4,7 @@
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import math
|
import math
|
||||||
|
import chelper
|
||||||
|
|
||||||
class DualCarriages:
|
class DualCarriages:
|
||||||
def __init__(self, printer, rail_0, rail_1, axis):
|
def __init__(self, printer, rail_0, rail_1, axis):
|
||||||
|
@ -24,6 +25,7 @@ class DualCarriages:
|
||||||
for i, dc in enumerate(self.dc):
|
for i, dc in enumerate(self.dc):
|
||||||
dc_rail = dc.get_rail()
|
dc_rail = dc.get_rail()
|
||||||
if i != index:
|
if i != index:
|
||||||
|
if dc.is_active():
|
||||||
dc.inactivate(pos)
|
dc.inactivate(pos)
|
||||||
kin.override_rail(3, dc_rail)
|
kin.override_rail(3, dc_rail)
|
||||||
elif dc.is_active() is False:
|
elif dc.is_active() is False:
|
||||||
|
@ -79,20 +81,43 @@ class DualCarriagesRail:
|
||||||
self.stepper_alloc_active = stepper_alloc_active
|
self.stepper_alloc_active = stepper_alloc_active
|
||||||
self.stepper_alloc_inactive = stepper_alloc_inactive
|
self.stepper_alloc_inactive = stepper_alloc_inactive
|
||||||
self.axis_position = -1
|
self.axis_position = -1
|
||||||
def _stepper_alloc(self, position, active=True):
|
self.stepper_active_sk = {}
|
||||||
|
self.stepper_inactive_sk = {}
|
||||||
|
for s in rail.get_steppers():
|
||||||
|
self._save_sk(self.status, s, s.get_stepper_kinematics())
|
||||||
|
def _alloc_sk(self, alloc_func, *params):
|
||||||
|
ffi_main, ffi_lib = chelper.get_ffi()
|
||||||
|
return ffi_main.gc(getattr(ffi_lib, alloc_func)(*params), ffi_lib.free)
|
||||||
|
def _get_sk(self, status, stepper):
|
||||||
|
sk = None
|
||||||
|
if status == self.ACTIVE:
|
||||||
|
sk = self.stepper_active_sk.get(stepper, None)
|
||||||
|
if sk is None and self.stepper_alloc_active:
|
||||||
|
sk = self._alloc_sk(*self.stepper_alloc_active)
|
||||||
|
self._save_sk(status, stepper, sk)
|
||||||
|
elif status == self.INACTIVE:
|
||||||
|
sk = self.stepper_inactive_sk.get(stepper, None)
|
||||||
|
if sk is None and self.stepper_alloc_inactive:
|
||||||
|
sk = self._alloc_sk(*self.stepper_alloc_inactive)
|
||||||
|
self._save_sk(status, stepper, sk)
|
||||||
|
return sk
|
||||||
|
def _save_sk(self, status, stepper, sk):
|
||||||
|
if status == self.ACTIVE:
|
||||||
|
self.stepper_active_sk[stepper] = sk
|
||||||
|
elif status == self.INACTIVE:
|
||||||
|
self.stepper_inactive_sk[stepper] = sk
|
||||||
|
def _update_stepper_alloc(self, position, active=True):
|
||||||
toolhead = self.printer.lookup_object('toolhead')
|
toolhead = self.printer.lookup_object('toolhead')
|
||||||
self.axis_position = position[self.axis]
|
self.axis_position = position[self.axis]
|
||||||
self.rail.set_trapq(None)
|
self.rail.set_trapq(None)
|
||||||
if active is True:
|
old_status = self.status
|
||||||
self.status = self.ACTIVE
|
self.status = (self.INACTIVE, self.ACTIVE)[active]
|
||||||
if self.stepper_alloc_active is not None:
|
for s in self.rail.get_steppers():
|
||||||
self.rail.setup_itersolve(*self.stepper_alloc_active)
|
sk = self._get_sk(self.status, s)
|
||||||
self.rail.set_position(position)
|
if sk is None:
|
||||||
self.rail.set_trapq(toolhead.get_trapq())
|
return
|
||||||
else:
|
old_sk = s.set_stepper_kinematics(sk)
|
||||||
self.status = self.INACTIVE
|
self._save_sk(old_status, s, old_sk)
|
||||||
if self.stepper_alloc_inactive is not None:
|
|
||||||
self.rail.setup_itersolve(*self.stepper_alloc_inactive)
|
|
||||||
self.rail.set_position(position)
|
self.rail.set_position(position)
|
||||||
self.rail.set_trapq(toolhead.get_trapq())
|
self.rail.set_trapq(toolhead.get_trapq())
|
||||||
def get_rail(self):
|
def get_rail(self):
|
||||||
|
@ -100,6 +125,6 @@ class DualCarriagesRail:
|
||||||
def is_active(self):
|
def is_active(self):
|
||||||
return self.status == self.ACTIVE
|
return self.status == self.ACTIVE
|
||||||
def activate(self, position):
|
def activate(self, position):
|
||||||
self._stepper_alloc(position, active=True)
|
self._update_stepper_alloc(position, active=True)
|
||||||
def inactivate(self, position):
|
def inactivate(self, position):
|
||||||
self._stepper_alloc(position, active=False)
|
self._update_stepper_alloc(position, active=False)
|
||||||
|
|
|
@ -160,6 +160,8 @@ class MCU_stepper:
|
||||||
count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count,
|
count = ffi_lib.stepcompress_extract_old(self._stepqueue, data, count,
|
||||||
start_clock, end_clock)
|
start_clock, end_clock)
|
||||||
return (data, count)
|
return (data, count)
|
||||||
|
def get_stepper_kinematics(self):
|
||||||
|
return self._stepper_kinematics
|
||||||
def set_stepper_kinematics(self, sk):
|
def set_stepper_kinematics(self, sk):
|
||||||
old_sk = self._stepper_kinematics
|
old_sk = self._stepper_kinematics
|
||||||
mcu_pos = 0
|
mcu_pos = 0
|
||||||
|
|
Loading…
Reference in New Issue