diff options
Diffstat (limited to 'klippy/mcu.py')
| -rw-r--r-- | klippy/mcu.py | 40 |
1 files changed, 28 insertions, 12 deletions
diff --git a/klippy/mcu.py b/klippy/mcu.py index f5d7489d..be04308b 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -28,6 +28,8 @@ class MCU_stepper: self._step_pin, pullup, self._invert_step = parse_pin_extras(step_pin) self._dir_pin, pullup, self._invert_dir = parse_pin_extras(dir_pin) self.commanded_position = 0 + self._step_dist = self._inv_step_dist = 1. + self._velocity_factor = self._inv_accel_factor = 0. self._mcu_position_offset = 0 self._mcu_freq = self._min_stop_interval = 0. self._reset_cmd = self._get_position_cmd = None @@ -35,8 +37,13 @@ class MCU_stepper: self.print_to_mcu_time = mcu.print_to_mcu_time def set_min_stop_interval(self, min_stop_interval): self._min_stop_interval = min_stop_interval + def set_step_distance(self, step_dist): + self._step_dist = step_dist + self._inv_step_dist = 1. / step_dist def build_config(self): self._mcu_freq = self._mcu.get_mcu_freq() + self._velocity_factor = 1. / (self._mcu_freq * self._step_dist) + self._inv_accel_factor = self._mcu_freq**2 * self._step_dist max_error = self._mcu.get_max_stepper_error() min_stop_interval = max(0., self._min_stop_interval - max_error) self._mcu.add_config_cmd( @@ -105,24 +112,33 @@ class MCU_stepper: self.commanded_position += 1 else: self.commanded_position -= 1 - def step_sqrt(self, mcu_time, steps, step_offset, sqrt_offset, factor): - clock = mcu_time * self._mcu_freq - mcu_freq2 = self._mcu_freq**2 - count = self._ffi_lib.stepcompress_push_sqrt( - self._stepqueue, steps, step_offset, clock - , sqrt_offset * mcu_freq2, factor * mcu_freq2) + def step_const(self, mcu_time, start_pos, dist, cruise_v): + #t = pos/cruise_v + inv_step_dist = self._inv_step_dist + step_offset = self.commanded_position - start_pos * inv_step_dist + steps = dist * inv_step_dist + count = self._ffi_lib.stepcompress_push_factor( + self._stepqueue, steps, step_offset, + mcu_time * self._mcu_freq, 1. / (cruise_v * self._velocity_factor)) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self.commanded_position += count - return count - def step_factor(self, mcu_time, steps, step_offset, factor): - clock = mcu_time * self._mcu_freq - count = self._ffi_lib.stepcompress_push_factor( - self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) + def step_accel(self, mcu_time, start_pos, dist, start_v, accel): + #t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel + inv_step_dist = self._inv_step_dist + mcu_freq = self._mcu_freq + inv_accel = 1. / accel + time_offset = start_v * inv_accel * mcu_freq + sqrt_offset = time_offset**2 + step_offset = self.commanded_position - start_pos * inv_step_dist + steps = dist * inv_step_dist + clock = mcu_time * mcu_freq - time_offset + count = self._ffi_lib.stepcompress_push_sqrt( + self._stepqueue, steps, step_offset, clock, + sqrt_offset, 2. * inv_accel * self._inv_accel_factor) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self.commanded_position += count - return count def step_delta_const(self, mcu_time, dist, start_pos , inv_velocity, step_dist , height, closestxy_d, closest_height2, movez_r): |