diff options
| -rw-r--r-- | klippy/chelper/__init__.py | 3 | ||||
| -rw-r--r-- | klippy/chelper/itersolve.c | 11 | ||||
| -rw-r--r-- | klippy/mcu.py | 29 |
3 files changed, 23 insertions, 20 deletions
diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index fd8b60a6..027cf277 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -49,9 +49,10 @@ defs_itersolve = """ , double axes_d_x, double axes_d_y, double axes_d_z , double start_v, double cruise_v, double accel); int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m); - void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos); void itersolve_set_stepcompress(struct stepper_kinematics *sk , struct stepcompress *sc, double step_dist); + void itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos); + double itersolve_get_commanded_pos(struct stepper_kinematics *sk); """ defs_kin_cartesian = """ diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c index 72e6ad89..c9ce9b8f 100644 --- a/klippy/chelper/itersolve.c +++ b/klippy/chelper/itersolve.c @@ -149,7 +149,7 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) double mcu_freq = stepcompress_get_mcu_freq(sc); struct timepos last = { 0., sk->commanded_pos }, low = last, high = last; double seek_time_delta = 0.000100; - int steps = 0, sdir = stepcompress_get_step_dir(sc); + int sdir = stepcompress_get_step_dir(sc); struct queue_append qa = queue_append_start(sc, m->print_time, .5); for (;;) { // Determine if next step is in forward or reverse direction @@ -192,7 +192,6 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) int ret = queue_append(&qa, next.time * mcu_freq); if (ret) return ret; - steps += sdir ? 1 : -1; seek_time_delta = next.time - last.time; if (seek_time_delta < .000000001) seek_time_delta = .000000001; @@ -205,7 +204,7 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m) } queue_append_finish(qa); sk->commanded_pos = last.position; - return steps; + return 0; } void __visible @@ -221,3 +220,9 @@ itersolve_set_commanded_pos(struct stepper_kinematics *sk, double pos) { sk->commanded_pos = pos; } + +double __visible +itersolve_get_commanded_pos(struct stepper_kinematics *sk) +{ + return sk->commanded_pos; +} 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): |