mcu: Store the stepper position in the mcu_stepper class

Move the storage of the stepper location from the kinematic classes to
the low-level mcu_stepper class.

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>

4 files changed, 38 insertions, 44 deletions

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
+            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
-            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)
             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)