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

mcu: Pass delta velocity and acceleration directly to mcu_stepper 

Rework the parameters of step_delta_const() and step_delta_accel() so
that they take velocity and acceleration directly in millimeters and
seconds.  This simplifies the delta.py kinematic code.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-04 19:03:04 -04:00
parent c4b1a79db2
commit 8c712d959d
2 changed files with 47 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
klippy/delta.py
View File

@ -183,19 +183,13 @@ class DeltaKinematics:
origx, origy, origz = move.start_pos[:3] origx, origy, origz = move.start_pos[:3]
accel = move.accel
cruise_v = move.cruise_v
accel_t = move.accel_t accel_t = move.accel_t
cruise_end_t = accel_t + move.cruise_t cruise_end_t = accel_t + move.cruise_t
accel_d = move.accel_r * move_d accel_d = move.accel_r * move_d
cruise_end_d = accel_d + move.cruise_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: for i in StepList:
# Find point on line of movement closest to tower # Find point on line of movement closest to tower
towerx_d = self.towers[i][0] - origx towerx_d = self.towers[i][0] - origx
@ -235,44 +229,32 @@ class DeltaKinematics:
# Generate steps # Generate steps
mcu_stepper = self.steppers[i].mcu_stepper mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time) 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.: if accel_up_d > 0.:
count = mcu_stepper.step_delta_accel( mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, accel_up_d, mcu_time, 0., accel_up_d, move.start_v, accel,
accel_offset, accel_multiplier, step_dist, origz, closestxy_d, closest_height2, movez_r)
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if cruise_up_d > 0.: if cruise_up_d > 0.:
count = mcu_stepper.step_delta_const( mcu_stepper.step_delta_const(
mcu_time + accel_t, cruise_up_d, mcu_time + accel_t, accel_d, cruise_up_d, cruise_v,
-accel_d, inv_cruise_v, step_dist, origz, closestxy_d, closest_height2, movez_r)
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if decel_up_d > 0.: if decel_up_d > 0.:
count = mcu_stepper.step_delta_accel( mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, decel_up_d, mcu_time + cruise_end_t, cruise_end_d, decel_up_d,
-decel_offset, -accel_multiplier, step_dist, cruise_v, -accel,
height, closestxy_d, closest_height2, movez_r) origz, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if accel_down_d > 0.: if accel_down_d > 0.:
count = mcu_stepper.step_delta_accel( mcu_stepper.step_delta_accel(
mcu_time - accel_time_offset, accel_down_d, mcu_time, 0., -accel_down_d, move.start_v, accel,
accel_offset, accel_multiplier, -step_dist, origz, closestxy_d, closest_height2, movez_r)
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if cruise_down_d > 0.: if cruise_down_d > 0.:
count = mcu_stepper.step_delta_const( mcu_stepper.step_delta_const(
mcu_time + accel_t, cruise_down_d, mcu_time + accel_t, accel_d, -cruise_down_d, cruise_v,
-accel_d, inv_cruise_v, -step_dist, origz, closestxy_d, closest_height2, movez_r)
height, closestxy_d, closest_height2, movez_r)
height += count * step_dist
if decel_down_d > 0.: if decel_down_d > 0.:
count = mcu_stepper.step_delta_accel( mcu_stepper.step_delta_accel(
mcu_time + decel_time_offset, decel_down_d, mcu_time + cruise_end_t, cruise_end_d, -decel_down_d,
-decel_offset, -accel_multiplier, -step_dist, cruise_v, -accel,
height, closestxy_d, closest_height2, movez_r) origz, closestxy_d, closest_height2, movez_r)
###################################################################### ######################################################################

40
klippy/mcu.py
View File

@ -139,31 +139,41 @@ class MCU_stepper:
if count == STEPCOMPRESS_ERROR_RET: if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
def step_delta_const(self, mcu_time, dist, start_pos def step_delta_const(self, mcu_time, start_pos, dist, cruise_v
, inv_velocity, step_dist , height_base, closestxy_d, closest_height2, movez_r):
, height, closestxy_d, closest_height2, movez_r): mcu_freq = self._mcu_freq
clock = mcu_time * 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( count = self._ffi_lib.stepcompress_push_delta_const(
self._stepqueue, clock, dist, start_pos self._stepqueue, mcu_time * mcu_freq, dist, -start_pos
, inv_velocity * self._mcu_freq, step_dist , mcu_freq / cruise_v, step_dist
, height, closestxy_d, closest_height2, movez_r) , height, closestxy_d, closest_height2, movez_r)
if count == STEPCOMPRESS_ERROR_RET: if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
return count def step_delta_accel(self, mcu_time, start_pos, dist, start_v, accel
def step_delta_accel(self, mcu_time, dist, start_pos , height_base, closestxy_d, closest_height2, movez_r):
, accel_multiplier, step_dist inv_step_dist = self._inv_step_dist
, height, closestxy_d, closest_height2, movez_r): mcu_freq = self._mcu_freq
clock = mcu_time * self._mcu_freq inv_accel = 1. / accel
mcu_freq2 = self._mcu_freq**2 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( count = self._ffi_lib.stepcompress_push_delta_accel(
self._stepqueue, clock, dist, start_pos self._stepqueue, clock, dist, accel_offset - start_pos
, accel_multiplier * mcu_freq2, step_dist , 2. * inv_accel * mcu_freq**2, step_dist
, height, closestxy_d, closest_height2, movez_r) , height, closestxy_d, closest_height2, movez_r)
if count == STEPCOMPRESS_ERROR_RET: if count == STEPCOMPRESS_ERROR_RET:
raise error("Internal error in stepcompress") raise error("Internal error in stepcompress")
self.commanded_position += count self.commanded_position += count
return count
class MCU_endstop: class MCU_endstop:
error = error error = error