diff options
Diffstat (limited to 'klippy/mcu.py')
| -rw-r--r-- | klippy/mcu.py | 29 |
1 files changed, 13 insertions, 16 deletions
diff --git a/klippy/mcu.py b/klippy/mcu.py index be97dc99..1969ff82 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -18,8 +18,8 @@ class MCU_stepper: self._step_pin = pin_params['pin'] self._invert_step = pin_params['invert'] self._dir_pin = self._invert_dir = None - self._commanded_pos = self._mcu_position_offset = 0. - self._step_dist = self._inv_step_dist = 1. + self._mcu_position_offset = 0. + self._step_dist = 0. self._min_stop_interval = 0. self._reset_cmd_id = self._get_position_cmd = None ffi_main, self._ffi_lib = chelper.get_ffi() @@ -39,7 +39,6 @@ class MCU_stepper: self._min_stop_interval = min_stop_interval def setup_step_distance(self, step_dist): self._step_dist = step_dist - self._inv_step_dist = 1. / step_dist def setup_itersolve(self, sk): old_sk = self._stepper_kinematics self._stepper_kinematics = sk @@ -73,16 +72,14 @@ class MCU_stepper: def get_step_dist(self): return self._step_dist def set_position(self, pos): - if self._stepper_kinematics is not None: - self._ffi_lib.itersolve_set_commanded_pos( - self._stepper_kinematics, pos) - steppos = pos * self._inv_step_dist - self._mcu_position_offset += self._commanded_pos - steppos - self._commanded_pos = steppos + self._mcu_position_offset += self.get_commanded_position() - pos + self._ffi_lib.itersolve_set_commanded_pos(self._stepper_kinematics, pos) def get_commanded_position(self): - return self._commanded_pos * self._step_dist + return self._ffi_lib.itersolve_get_commanded_pos( + self._stepper_kinematics) def get_mcu_position(self): - mcu_pos = self._commanded_pos + self._mcu_position_offset + pos_delta = self.get_commanded_position() + self._mcu_position_offset + mcu_pos = pos_delta / self._step_dist if mcu_pos >= 0.: return int(mcu_pos + 0.5) return int(mcu_pos - 0.5) @@ -115,15 +112,15 @@ class MCU_stepper: return params = self._get_position_cmd.send_with_response( [self._oid], response='stepper_position', response_oid=self._oid) - pos = params['pos'] + pos = params['pos'] * self._step_dist if self._invert_dir: pos = -pos - self._commanded_pos = pos - self._mcu_position_offset + self._ffi_lib.itersolve_set_commanded_pos( + self._stepper_kinematics, pos - self._mcu_position_offset) def step_itersolve(self, cmove): - count = self._itersolve_gen_steps(self._stepper_kinematics, cmove) - if count == STEPCOMPRESS_ERROR_RET: + ret = self._itersolve_gen_steps(self._stepper_kinematics, cmove) + if ret: raise error("Internal error in stepcompress") - self._commanded_pos += count class MCU_endstop: class TimeoutError(Exception): |