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

mcu: Pass constant velocity and acceleration directly to mcu_stepper 

Rename step_sqrt/step_factor to step_accel/step_const and have them
directly take the velocity and acceleration in millimeters and
seconds.  This simplifies the kinematic classes.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
Kevin O'Connor 2017-04-04 12:31:03 -04:00
parent 47f12f107d
commit c4b1a79db2
5 changed files with 82 additions and 121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
klippy/cartesian.py
View File

@ -99,46 +99,32 @@ class CartKinematics:
def move(self, move_time, move):
if self.need_motor_enable:
self._check_motor_enable(move_time, move)
inv_accel = 1. / move.accel
inv_cruise_v = 1. / move.cruise_v
for i in StepList:
if not move.axes_d[i]:
axis_d = move.axes_d[i]
if not axis_d:
continue
mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
step_pos = mcu_stepper.commanded_position
inv_step_dist = self.steppers[i].inv_step_dist
step_offset = step_pos - move.start_pos[i] * inv_step_dist
steps = move.axes_d[i] * inv_step_dist
move_step_d = move.move_d / abs(steps)
start_pos = move.start_pos[i]
axis_r = abs(axis_d) / move.move_d
accel = move.accel * axis_r
cruise_v = move.cruise_v * axis_r
# Acceleration steps
accel_multiplier = 2.0 * move_step_d * inv_accel
if move.accel_r:
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = move.start_v * inv_accel
accel_sqrt_offset = accel_time_offset**2
accel_steps = move.accel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
step_offset += count - accel_steps
accel_d = move.accel_r * axis_d
mcu_stepper.step_accel(
mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
start_pos += accel_d
mcu_time += move.accel_t
# Cruising steps
if move.cruise_r:
#t = pos/cruise_v
cruise_multiplier = move_step_d * inv_cruise_v
cruise_steps = move.cruise_r * steps
count = mcu_stepper.step_factor(
mcu_time, cruise_steps, step_offset, cruise_multiplier)
step_offset += count - cruise_steps
cruise_d = move.cruise_r * axis_d
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
start_pos += cruise_d
mcu_time += move.cruise_t
# Deceleration steps
if move.decel_r:
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_time_offset = move.cruise_v * inv_accel
decel_sqrt_offset = decel_time_offset**2
decel_steps = move.decel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier)
decel_d = move.decel_r * axis_d
mcu_stepper.step_accel(
mcu_time, start_pos, decel_d, cruise_v, -accel)

53
klippy/corexy.py
View File

@ -111,54 +111,39 @@ class CoreXYKinematics:
def move(self, move_time, move):
if self.need_motor_enable:
self._check_motor_enable(move_time, move)
inv_accel = 1. / move.accel
inv_cruise_v = 1. / move.cruise_v
sxp = move.start_pos[0]
syp = move.start_pos[1]
start_pos = (sxp + syp, sxp - syp, move.start_pos[2])
move_start_pos = (sxp + syp, sxp - syp, move.start_pos[2])
exp = move.end_pos[0]
eyp = move.end_pos[1]
end_pos = (exp + eyp, exp - eyp, move.start_pos[2])
axes_d = (end_pos[0] - start_pos[0], end_pos[1] - start_pos[1],
move.axes_d[2])
axes_d = ((exp + eyp) - move_start_pos[0],
(exp - eyp) - move_start_pos[1], move.axes_d[2])
for i in StepList:
if not axes_d[i]:
axis_d = axes_d[i]
if not axis_d:
continue
mcu_stepper = self.steppers[i].mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
step_pos = mcu_stepper.commanded_position
inv_step_dist = self.steppers[i].inv_step_dist
step_offset = step_pos - start_pos[i] * inv_step_dist
steps = axes_d[i] * inv_step_dist
move_step_d = move.move_d / abs(steps)
start_pos = move_start_pos[i]
axis_r = abs(axis_d) / move.move_d
accel = move.accel * axis_r
cruise_v = move.cruise_v * axis_r
# Acceleration steps
accel_multiplier = 2.0 * move_step_d * inv_accel
if move.accel_r:
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = move.start_v * inv_accel
accel_sqrt_offset = accel_time_offset**2
accel_steps = move.accel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
step_offset += count - accel_steps
accel_d = move.accel_r * axis_d
mcu_stepper.step_accel(
mcu_time, start_pos, accel_d, move.start_v * axis_r, accel)
start_pos += accel_d
mcu_time += move.accel_t
# Cruising steps
if move.cruise_r:
#t = pos/cruise_v
cruise_multiplier = move_step_d * inv_cruise_v
cruise_steps = move.cruise_r * steps
count = mcu_stepper.step_factor(
mcu_time, cruise_steps, step_offset, cruise_multiplier)
step_offset += count - cruise_steps
cruise_d = move.cruise_r * axis_d
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
start_pos += cruise_d
mcu_time += move.cruise_t
# Deceleration steps
if move.decel_r:
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_time_offset = move.cruise_v * inv_accel
decel_sqrt_offset = decel_time_offset**2
decel_steps = move.decel_r * steps
count = mcu_stepper.step_sqrt(
mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier)
decel_d = move.decel_r * axis_d
mcu_stepper.step_accel(
mcu_time, start_pos, decel_d, cruise_v, -accel)

59
klippy/extruder.py
View File

@ -111,12 +111,11 @@ class PrinterExtruder:
self.stepper.motor_enable(move_time, 1)
self.need_motor_enable = False
axis_d = move.axes_d[3]
extrude_r = abs(axis_d) / move.move_d
inv_accel = 1. / (move.accel * extrude_r)
start_v = move.start_v * extrude_r
cruise_v = move.cruise_v * extrude_r
end_v = move.end_v * extrude_r
axis_r = abs(axis_d) / move.move_d
accel = move.accel * axis_r
start_v = move.start_v * axis_r
cruise_v = move.cruise_v * axis_r
end_v = move.end_v * axis_r
accel_t, cruise_t, decel_t = move.accel_t, move.cruise_t, move.decel_t
accel_d = move.accel_r * axis_d
cruise_d = move.cruise_r * axis_d
@ -156,7 +155,7 @@ class PrinterExtruder:
decel_t = decel_d = 0.
elif end_v < 0.:
# Split decel phase into decel and retraction
retract_t = -end_v * inv_accel
retract_t = -end_v / accel
retract_d = -end_v * 0.5 * retract_t
decel_t -= retract_t
decel_d = decel_v * 0.5 * decel_t
@ -165,56 +164,30 @@ class PrinterExtruder:
decel_d -= extra_decel_d
# Prepare for steps
inv_step_dist = self.stepper.inv_step_dist
step_dist = self.stepper.step_dist
mcu_stepper = self.stepper.mcu_stepper
mcu_time = mcu_stepper.print_to_mcu_time(move_time)
step_pos = mcu_stepper.commanded_position
step_offset = step_pos - start_pos * inv_step_dist
# Acceleration steps
accel_multiplier = 2.0 * step_dist * inv_accel
if accel_d:
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = start_v * inv_accel
accel_sqrt_offset = accel_time_offset**2
accel_steps = accel_d * inv_step_dist
count = mcu_stepper.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
step_offset += count - accel_steps
mcu_stepper.step_accel(mcu_time, start_pos, accel_d, start_v, accel)
start_pos += accel_d
mcu_time += accel_t
# Cruising steps
if cruise_d:
#t = pos/cruise_v
cruise_multiplier = step_dist / cruise_v
cruise_steps = cruise_d * inv_step_dist
count = mcu_stepper.step_factor(
mcu_time, cruise_steps, step_offset, cruise_multiplier)
step_offset += count - cruise_steps
mcu_stepper.step_const(mcu_time, start_pos, cruise_d, cruise_v)
start_pos += cruise_d
mcu_time += cruise_t
# Deceleration steps
if decel_d:
#t = cruise_v/accel - sqrt((cruise_v/accel)**2 - 2*pos/accel)
decel_time_offset = decel_v * inv_accel
decel_sqrt_offset = decel_time_offset**2
decel_steps = decel_d * inv_step_dist
count = mcu_stepper.step_sqrt(
mcu_time + decel_time_offset, decel_steps, step_offset
, decel_sqrt_offset, -accel_multiplier)
step_offset += count - decel_steps
mcu_stepper.step_accel(mcu_time, start_pos, decel_d, decel_v, -accel)
start_pos += decel_d
mcu_time += decel_t
# Retraction steps
if retract_d:
#t = sqrt(2*pos/accel + (start_v/accel)**2) - start_v/accel
accel_time_offset = retract_v * inv_accel
accel_sqrt_offset = accel_time_offset**2
accel_steps = -retract_d * inv_step_dist
count = mcu_stepper.step_sqrt(
mcu_time - accel_time_offset, accel_steps, step_offset
, accel_sqrt_offset, accel_multiplier)
self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d
mcu_stepper.step_accel(
mcu_time, start_pos, -retract_d, retract_v, accel)
start_pos -= retract_d
self.extrude_pos = start_pos
# Dummy extruder class used when a printer has no extruder at all
class DummyExtruder:

44
klippy/mcu.py
View File

@ -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)
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
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, clock, factor * self._mcu_freq)
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
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):

1
klippy/stepper.py
View File

@ -43,6 +43,7 @@ class PrinterStepper:
dir_pin = config.get('dir_pin')
mcu = printer.mcu
self.mcu_stepper = mcu.create_stepper(step_pin, dir_pin)
self.mcu_stepper.set_step_distance(self.step_dist)
enable_pin = config.get('enable_pin', None)
if enable_pin is not None:
self.mcu_enable = mcu.create_digital_out(enable_pin, 0)