diff --git a/klippy/cartesian.py b/klippy/cartesian.py index 4907f58a..f6fe86bb 100644 --- a/klippy/cartesian.py +++ b/klippy/cartesian.py @@ -15,15 +15,15 @@ class CartKinematics: for n in ['x', 'y', 'z']] self.need_motor_enable = True self.limits = [(1.0, -1.0)] * 3 - self.stepper_pos = [0, 0, 0] def build_config(self): for stepper in self.steppers[:2]: stepper.set_max_jerk(0.005 * stepper.max_accel) # XXX for stepper in self.steppers: stepper.build_config() def set_position(self, newpos): - self.stepper_pos = [int(newpos[i]*self.steppers[i].inv_step_dist + 0.5) - for i in StepList] + for i in StepList: + s = self.steppers[i] + s.mcu_stepper.set_position(int(newpos[i]*s.inv_step_dist + 0.5)) def get_max_speed(self): max_xy_speed = min(s.max_velocity for s in self.steppers[:2]) max_xy_accel = min(s.max_accel for s in self.steppers[:2]) @@ -115,13 +115,12 @@ class CartKinematics: 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) - step_pos = self.stepper_pos[i] - step_offset = step_pos - move.start_pos[i] * inv_step_dist - # Acceleration steps accel_multiplier = 2.0 * move_step_d * inv_accel if move.accel_r: @@ -132,7 +131,6 @@ class CartKinematics: count = mcu_stepper.step_sqrt( mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - step_pos += count step_offset += count - accel_steps mcu_time += move.accel_t # Cruising steps @@ -142,7 +140,6 @@ class CartKinematics: cruise_steps = move.cruise_r * steps count = mcu_stepper.step_factor( mcu_time, cruise_steps, step_offset, cruise_multiplier) - step_pos += count step_offset += count - cruise_steps mcu_time += move.cruise_t # Deceleration steps @@ -154,5 +151,3 @@ class CartKinematics: count = mcu_stepper.step_sqrt( mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) - step_pos += count - self.stepper_pos[i] = step_pos diff --git a/klippy/delta.py b/klippy/delta.py index 9b12cb8a..ca46534c 100644 --- a/klippy/delta.py +++ b/klippy/delta.py @@ -28,7 +28,7 @@ class DeltaKinematics: (cos(210.)*radius, sin(210.)*radius), (cos(330.)*radius, sin(330.)*radius), (cos(90.)*radius, sin(90.)*radius)] - self.stepper_pos = self.cartesian_to_actuator([0., 0., 0.]) + self.set_position([0., 0., 0.]) def build_config(self): for stepper in self.steppers: stepper.set_max_jerk(0.005 * stepper.max_accel) # XXX @@ -77,11 +77,13 @@ class DeltaKinematics: return matrix_sub(circumcenter, matrix_mul(normal, dist)) def set_position(self, newpos): - self.stepper_pos = self.cartesian_to_actuator(newpos) + pos = self.cartesian_to_actuator(newpos) + for i in StepList: + self.steppers[i].mcu_stepper.set_position(pos[i]) def get_homed_position(self, homing_state): - pos = [(self.stepper_pos[i] + self.steppers[i].get_homed_offset()) - * self.steppers[i].step_dist - for i in StepList] + pos = [(s.mcu_stepper.commanded_position + s.get_homed_offset()) + * s.step_dist + for s in self.steppers] return self.actuator_to_cartesian(pos) def home(self, homing_state): # All axes are homed simultaneously @@ -138,13 +140,12 @@ class DeltaKinematics: 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 - height * inv_step_dist step_dist = self.steppers[i].step_dist steps = move.axes_d[2] * inv_step_dist - step_pos = self.stepper_pos[i] - step_offset = step_pos - height * inv_step_dist - # Acceleration steps accel_multiplier = 2.0 * step_dist * inv_accel if move.accel_r: @@ -155,7 +156,6 @@ class DeltaKinematics: count = mcu_stepper.step_sqrt( mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - step_pos += count step_offset += count - accel_steps mcu_time += move.accel_t # Cruising steps @@ -165,7 +165,6 @@ class DeltaKinematics: cruise_steps = move.cruise_r * steps count = mcu_stepper.step_factor( mcu_time, cruise_steps, step_offset, cruise_multiplier) - step_pos += count step_offset += count - cruise_steps mcu_time += move.cruise_t # Deceleration steps @@ -177,8 +176,6 @@ class DeltaKinematics: count = mcu_stepper.step_sqrt( mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) - step_pos += count - self.stepper_pos[i] = step_pos def move(self, move_time, move): axes_d = move.axes_d if not axes_d[0] and not axes_d[1]: @@ -250,54 +247,47 @@ class DeltaKinematics: decel_down_d = move_d # Generate steps - inv_step_dist = self.steppers[i].inv_step_dist - step_dist = self.steppers[i].step_dist - step_pos = self.stepper_pos[i] - height = step_pos*step_dist - closestz 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_dist = self.steppers[i].step_dist + height = step_pos*step_dist - closestz if accel_up_d > 0.: count = mcu_stepper.step_delta_accel( mcu_time - accel_time_offset, closest_d - accel_up_d, step_dist, closest_d + accel_offset, closest_height2, height, movez_r, accel_multiplier) - step_pos += count height += count * step_dist if cruise_up_d > 0.: count = mcu_stepper.step_delta_const( mcu_time + accel_t, closest_d - cruise_up_d, step_dist, closest_d - accel_d, closest_height2, height, movez_r, inv_cruise_v) - step_pos += count height += count * step_dist if decel_up_d > 0.: count = mcu_stepper.step_delta_accel( mcu_time + decel_time_offset, closest_d - decel_up_d, step_dist, closest_d - decel_offset, closest_height2, height, movez_r, -accel_multiplier) - step_pos += count height += count * step_dist if accel_down_d > 0.: count = mcu_stepper.step_delta_accel( mcu_time - accel_time_offset, closest_d - accel_down_d, -step_dist, closest_d + accel_offset, closest_height2, height, movez_r, accel_multiplier) - step_pos += count height += count * step_dist if cruise_down_d > 0.: count = mcu_stepper.step_delta_const( mcu_time + accel_t, closest_d - cruise_down_d, -step_dist, closest_d - accel_d, closest_height2, height, movez_r, inv_cruise_v) - step_pos += count height += count * step_dist if decel_down_d > 0.: count = mcu_stepper.step_delta_accel( mcu_time + decel_time_offset, closest_d - decel_down_d, -step_dist, closest_d - decel_offset, closest_height2, height, movez_r, -accel_multiplier) - step_pos += count - self.stepper_pos[i] = step_pos ###################################################################### diff --git a/klippy/extruder.py b/klippy/extruder.py index 823ed599..0ca57eba 100644 --- a/klippy/extruder.py +++ b/klippy/extruder.py @@ -12,7 +12,6 @@ class PrinterExtruder: self.stepper = stepper.PrinterStepper(printer, config, 'extruder') self.pressure_advance = config.getfloat('pressure_advance', 0.) self.need_motor_enable = True - self.stepper_pos = 0 self.extrude_pos = 0. def build_config(self): self.heater.build_config() @@ -95,12 +94,12 @@ class PrinterExtruder: decel_d -= extra_decel_d # Prepare for steps - stepper_pos = self.stepper_pos 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_offset = stepper_pos - start_pos * inv_step_dist + 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 @@ -112,7 +111,6 @@ class PrinterExtruder: count = mcu_stepper.step_sqrt( mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - stepper_pos += count step_offset += count - accel_steps mcu_time += accel_t # Cruising steps @@ -122,7 +120,6 @@ class PrinterExtruder: cruise_steps = cruise_d * inv_step_dist count = mcu_stepper.step_factor( mcu_time, cruise_steps, step_offset, cruise_multiplier) - stepper_pos += count step_offset += count - cruise_steps mcu_time += cruise_t # Deceleration steps @@ -134,7 +131,6 @@ class PrinterExtruder: count = mcu_stepper.step_sqrt( mcu_time + decel_time_offset, decel_steps, step_offset , decel_sqrt_offset, -accel_multiplier) - stepper_pos += count step_offset += count - decel_steps mcu_time += decel_t # Retraction steps @@ -146,7 +142,5 @@ class PrinterExtruder: count = mcu_stepper.step_sqrt( mcu_time - accel_time_offset, accel_steps, step_offset , accel_sqrt_offset, accel_multiplier) - stepper_pos += count - self.stepper_pos = stepper_pos self.extrude_pos = start_pos + accel_d + cruise_d + decel_d - retract_d diff --git a/klippy/mcu.py b/klippy/mcu.py index 54cc78da..e10b82b3 100644 --- a/klippy/mcu.py +++ b/klippy/mcu.py @@ -31,6 +31,7 @@ class MCU_stepper: self._mcu_freq = mcu.get_mcu_freq() min_stop_interval = int(min_stop_interval * self._mcu_freq) max_error = int(max_error * self._mcu_freq) + self.commanded_position = 0 mcu.add_config_cmd( "config_stepper oid=%d step_pin=%s dir_pin=%s" " min_stop_interval=%d invert_step=%d" % ( @@ -51,6 +52,8 @@ class MCU_stepper: return self._oid def get_invert_dir(self): return self._invert_dir + def set_position(self, pos): + self.commanded_position = pos def note_stepper_stop(self): self.ffi_lib.stepcompress_reset(self._stepqueue, 0) def reset_step_clock(self, mcu_time): @@ -61,29 +64,41 @@ class MCU_stepper: def step(self, mcu_time, sdir): clock = mcu_time * self._mcu_freq self.ffi_lib.stepcompress_push(self._stepqueue, clock, sdir) + if sdir: + 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 - return self.ffi_lib.stepcompress_push_sqrt( + count = self.ffi_lib.stepcompress_push_sqrt( self._stepqueue, steps, step_offset, clock , sqrt_offset * mcu_freq2, factor * mcu_freq2) + self.commanded_position += count + return count def step_factor(self, mcu_time, steps, step_offset, factor): clock = mcu_time * self._mcu_freq - return self.ffi_lib.stepcompress_push_factor( + count = self.ffi_lib.stepcompress_push_factor( self._stepqueue, steps, step_offset, clock, factor * self._mcu_freq) + self.commanded_position += count + return count def step_delta_const(self, mcu_time, dist, step_dist, start_pos , closest_height2, height, movez_r, inv_velocity): clock = mcu_time * self._mcu_freq - return self.ffi_lib.stepcompress_push_delta_const( + count = self.ffi_lib.stepcompress_push_delta_const( self._stepqueue, clock, dist, step_dist, start_pos , closest_height2, height, movez_r, inv_velocity * self._mcu_freq) + self.commanded_position += count + return count def step_delta_accel(self, mcu_time, dist, step_dist, start_pos , closest_height2, height, movez_r, accel_multiplier): clock = mcu_time * self._mcu_freq mcu_freq2 = self._mcu_freq**2 - return self.ffi_lib.stepcompress_push_delta_accel( + count = self.ffi_lib.stepcompress_push_delta_accel( self._stepqueue, clock, dist, step_dist, start_pos , closest_height2, height, movez_r, accel_multiplier * mcu_freq2) + self.commanded_position += count + return count def get_errors(self): return self.ffi_lib.stepcompress_get_errors(self._stepqueue)