3diy
/
klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

stepper: Restore mcu_position on set_stepper_kinematics() and set_step_dist() 

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2021-06-11 21:38:09 -04:00
parent 1506043477
commit 0bc0767997
1 changed files with 20 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
klippy/stepper.py
View File

@ -84,8 +84,10 @@ class MCU_stepper:
def get_step_dist(self): def get_step_dist(self):
return self._step_dist return self._step_dist
def set_step_dist(self, dist): def set_step_dist(self, dist):
mcu_pos = self.get_mcu_position()
self._step_dist = dist self._step_dist = dist
self.set_stepper_kinematics(self._stepper_kinematics) self.set_stepper_kinematics(self._stepper_kinematics)
self._set_mcu_position(mcu_pos)
def is_dir_inverted(self): def is_dir_inverted(self):
return self._invert_dir return self._invert_dir
def calc_position_from_coord(self, coord): def calc_position_from_coord(self, coord):
@ -93,21 +95,23 @@ class MCU_stepper:
return ffi_lib.itersolve_calc_position_from_coord( return ffi_lib.itersolve_calc_position_from_coord(
self._stepper_kinematics, coord[0], coord[1], coord[2]) self._stepper_kinematics, coord[0], coord[1], coord[2])
def set_position(self, coord): def set_position(self, coord):
opos = self.get_commanded_position() mcu_pos = self.get_mcu_position()
sk = self._stepper_kinematics sk = self._stepper_kinematics
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
ffi_lib.itersolve_set_position(sk, coord[0], coord[1], coord[2]) ffi_lib.itersolve_set_position(sk, coord[0], coord[1], coord[2])
self._mcu_position_offset += opos - self.get_commanded_position() self._set_mcu_position(mcu_pos)
def get_commanded_position(self): def get_commanded_position(self):
sk = self._stepper_kinematics
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
return ffi_lib.itersolve_get_commanded_pos(sk) return ffi_lib.itersolve_get_commanded_pos(self._stepper_kinematics)
def get_mcu_position(self): def get_mcu_position(self):
mcu_pos_dist = self.get_commanded_position() + self._mcu_position_offset mcu_pos_dist = self.get_commanded_position() + self._mcu_position_offset
mcu_pos = mcu_pos_dist / self._step_dist mcu_pos = mcu_pos_dist / self._step_dist
if mcu_pos >= 0.: if mcu_pos >= 0.:
return int(mcu_pos + 0.5) return int(mcu_pos + 0.5)
return int(mcu_pos - 0.5) return int(mcu_pos - 0.5)
def _set_mcu_position(self, mcu_pos):
mcu_pos_dist = mcu_pos * self._step_dist
self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()
def get_past_mcu_position(self, print_time): def get_past_mcu_position(self, print_time):
clock = self._mcu.print_time_to_clock(print_time) clock = self._mcu.print_time_to_clock(print_time)
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
@ -117,12 +121,14 @@ class MCU_stepper:
return mcu_pos * self._step_dist - self._mcu_position_offset return mcu_pos * self._step_dist - self._mcu_position_offset
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
if old_sk is not None:
mcu_pos = self.get_mcu_position()
self._stepper_kinematics = sk self._stepper_kinematics = sk
if sk is not None: ffi_main, ffi_lib = chelper.get_ffi()
ffi_main, ffi_lib = chelper.get_ffi() ffi_lib.itersolve_set_stepcompress(sk, self._stepqueue, self._step_dist)
ffi_lib.itersolve_set_stepcompress(sk, self._stepqueue, self.set_trapq(self._trapq)
self._step_dist) self._set_mcu_position(mcu_pos)
self.set_trapq(self._trapq)
return old_sk return old_sk
def note_homing_end(self, did_trigger=False): def note_homing_end(self, did_trigger=False):
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
@ -142,8 +148,7 @@ class MCU_stepper:
ret = ffi_lib.stepcompress_set_last_position(self._stepqueue, last_pos) ret = ffi_lib.stepcompress_set_last_position(self._stepqueue, last_pos)
if ret: if ret:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
mcu_pos_dist = last_pos * self._step_dist self._set_mcu_position(last_pos)
self._mcu_position_offset = mcu_pos_dist - self.get_commanded_position()
def set_trapq(self, tq): def set_trapq(self, tq):
ffi_main, ffi_lib = chelper.get_ffi() ffi_main, ffi_lib = chelper.get_ffi()
if tq is None: if tq is None:
@ -157,16 +162,16 @@ class MCU_stepper:
def generate_steps(self, flush_time): def generate_steps(self, flush_time):
# Check for activity if necessary # Check for activity if necessary
if self._active_callbacks: if self._active_callbacks:
ret = self._itersolve_check_active(self._stepper_kinematics, sk = self._stepper_kinematics
flush_time) ret = self._itersolve_check_active(sk, flush_time)
if ret: if ret:
cbs = self._active_callbacks cbs = self._active_callbacks
self._active_callbacks = [] self._active_callbacks = []
for cb in cbs: for cb in cbs:
cb(ret) cb(ret)
# Generate steps # Generate steps
ret = self._itersolve_generate_steps(self._stepper_kinematics, sk = self._stepper_kinematics
flush_time) ret = self._itersolve_generate_steps(sk, flush_time)
if ret: if ret:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
def is_active_axis(self, axis): def is_active_axis(self, axis):