diff --git a/klippy/delta.py b/klippy/delta.py index 009fb7b6..9722ff3d 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -183,19 +183,13 @@ class DeltaKinematics: origx, origy, origz = move.start_pos[:3] + accel = move.accel + cruise_v = move.cruise_v accel_t = move.accel_t cruise_end_t = accel_t + move.cruise_t accel_d = move.accel_r * move_d cruise_end_d = accel_d + move.cruise_r * move_d - inv_cruise_v = 1. / move.cruise_v - inv_accel = 1. / move.accel - accel_time_offset = move.start_v * inv_accel - accel_multiplier = 2.0 * inv_accel - accel_offset = move.start_v**2 * 0.5 * inv_accel - decel_time_offset = move.cruise_v * inv_accel + cruise_end_t - decel_offset = move.cruise_v**2 * 0.5 * inv_accel + cruise_end_d - for i in StepList: # Find point on line of movement closest to tower towerx_d = self.towers[i][0] - origx @@ -235,44 +229,32 @@ class DeltaKinematics: # Generate steps mcu_stepper = self.steppers[i].mcu_stepper mcu_time = mcu_stepper.print_to_mcu_time(move_time) - step_pos = mcu_stepper.commanded_position - step_dist = self.steppers[i].step_dist - height = step_pos*step_dist - origz if accel_up_d > 0.: - count = mcu_stepper.step_delta_accel( - mcu_time - accel_time_offset, accel_up_d, - accel_offset, accel_multiplier, step_dist, - height, closestxy_d, closest_height2, movez_r) - height += count * step_dist + mcu_stepper.step_delta_accel( + mcu_time, 0., accel_up_d, move.start_v, accel, + origz, closestxy_d, closest_height2, movez_r) if cruise_up_d > 0.: - count = mcu_stepper.step_delta_const( - mcu_time + accel_t, cruise_up_d, - -accel_d, inv_cruise_v, step_dist, - height, closestxy_d, closest_height2, movez_r) - height += count * step_dist + mcu_stepper.step_delta_const( + mcu_time + accel_t, accel_d, cruise_up_d, cruise_v, + origz, closestxy_d, closest_height2, movez_r) if decel_up_d > 0.: - count = mcu_stepper.step_delta_accel( - mcu_time + decel_time_offset, decel_up_d, - -decel_offset, -accel_multiplier, step_dist, - height, closestxy_d, closest_height2, movez_r) - height += count * step_dist + mcu_stepper.step_delta_accel( + mcu_time + cruise_end_t, cruise_end_d, decel_up_d, + cruise_v, -accel, + origz, closestxy_d, closest_height2, movez_r) if accel_down_d > 0.: - count = mcu_stepper.step_delta_accel( - mcu_time - accel_time_offset, accel_down_d, - accel_offset, accel_multiplier, -step_dist, - height, closestxy_d, closest_height2, movez_r) - height += count * step_dist + mcu_stepper.step_delta_accel( + mcu_time, 0., -accel_down_d, move.start_v, accel, + origz, closestxy_d, closest_height2, movez_r) if cruise_down_d > 0.: - count = mcu_stepper.step_delta_const( - mcu_time + accel_t, cruise_down_d, - -accel_d, inv_cruise_v, -step_dist, - height, closestxy_d, closest_height2, movez_r) - height += count * step_dist + mcu_stepper.step_delta_const( + mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v, + origz, closestxy_d, closest_height2, movez_r) if decel_down_d > 0.: - count = mcu_stepper.step_delta_accel( - mcu_time + decel_time_offset, decel_down_d, - -decel_offset, -accel_multiplier, -step_dist, - height, closestxy_d, closest_height2, movez_r) + mcu_stepper.step_delta_accel( + mcu_time + cruise_end_t, cruise_end_d, -decel_down_d, + cruise_v, -accel, + origz, closestxy_d, closest_height2, movez_r) ###################################################################### diff --git a/klippy/mcu.py b/klippy/mcu.py index be04308b..5e51b14d 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -139,31 +139,41 @@ class MCU_stepper: if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self.commanded_position += count - def step_delta_const(self, mcu_time, dist, start_pos - , inv_velocity, step_dist - , height, closestxy_d, closest_height2, movez_r): - clock = mcu_time * self._mcu_freq + def step_delta_const(self, mcu_time, start_pos, dist, cruise_v + , height_base, closestxy_d, closest_height2, movez_r): + mcu_freq = self._mcu_freq + step_dist = self._step_dist + height = self.commanded_position*step_dist - height_base + if dist < 0: + dist = -dist + step_dist = -step_dist count = self._ffi_lib.stepcompress_push_delta_const( - self._stepqueue, clock, dist, start_pos - , inv_velocity * self._mcu_freq, step_dist + self._stepqueue, mcu_time * mcu_freq, dist, -start_pos + , mcu_freq / cruise_v, step_dist , height, closestxy_d, closest_height2, movez_r) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self.commanded_position += count - return count - def step_delta_accel(self, mcu_time, dist, start_pos - , accel_multiplier, step_dist - , height, closestxy_d, closest_height2, movez_r): - clock = mcu_time * self._mcu_freq - mcu_freq2 = self._mcu_freq**2 + def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel + , height_base, closestxy_d, closest_height2, movez_r): + inv_step_dist = self._inv_step_dist + mcu_freq = self._mcu_freq + inv_accel = 1. / accel + time_offset = start_v * inv_accel * mcu_freq + accel_offset = start_v**2 * 0.5 * inv_accel + step_dist = self._step_dist + height = self.commanded_position*step_dist - height_base + if dist < 0: + dist = -dist + step_dist = -step_dist + clock = mcu_time * mcu_freq - time_offset count = self._ffi_lib.stepcompress_push_delta_accel( - self._stepqueue, clock, dist, start_pos - , accel_multiplier * mcu_freq2, step_dist + self._stepqueue, clock, dist, accel_offset - start_pos + , 2. * inv_accel * mcu_freq**2, step_dist , height, closestxy_d, closest_height2, movez_r) if count == STEPCOMPRESS_ERROR_RET: raise error("Internal error in stepcompress") self.commanded_position += count - return count class MCU_endstop: error = error