diff options
| -rw-r--r-- | klippy/cartesian.py | 46 | ||||
| -rw-r--r-- | klippy/corexy.py | 53 | ||||
| -rw-r--r-- | klippy/extruder.py | 59 | ||||
| -rw-r--r-- | klippy/mcu.py | 40 | ||||
| -rw-r--r-- | klippy/stepper.py | 1 |
5 files changed, 80 insertions, 119 deletions
diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 2dc0df3d..6ec12a5b 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -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) diff --git a/klippy/corexy.py b/klippy/corexy.py index f8b2a052..e9454a2c 100644 --- a/klippy/corexy.py +++ b/klippy/corexy.py @@ -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) diff --git a/klippy/extruder.py b/klippy/extruder.py index b62244e8..4321f06b 100644 --- a/klippy/extruder.py +++ b/klippy/extruder.py @@ -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: 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): diff --git a/klippy/stepper.py b/klippy/stepper.py index eb8c2400..a5a10c7a 100644 --- a/klippy/stepper.py +++ b/klippy/stepper.py @@ -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) |