stepper: Check if the motor needs to be enabled in the kinematic classes

Check for motor enable in the kinematic classes so it doesn't need to
be checked on every move.

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

4 files changed, 55 insertions, 29 deletions

diff --git a/klippy/cartesian.py b/klippy/cartesian.py
index 85746860..e386704c 100644
--- a/klippy/cartesian.py
+++ b/klippy/cartesian.py
@@ -13,6 +13,7 @@ class CartKinematics:
         steppers = ['stepper_x', 'stepper_y', 'stepper_z']
         self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
                          for n in steppers]
+        self.need_motor_enable = True
         self.limits = [(1.0, -1.0)] * 3
         self.stepper_pos = [0, 0, 0]
     def build_config(self):
@@ -64,6 +65,14 @@ class CartKinematics:
         self.limits = [(1.0, -1.0)] * 3
         for stepper in self.steppers:
             stepper.motor_enable(move_time, 0)
+        self.need_motor_enable = True
+    def check_motor_enable(self, move_time, move):
+        need_motor_enable = False
+        for i in StepList:
+            if move.axes_d[i]:
+                self.steppers[i].motor_enable(move_time, 1)
+            need_motor_enable |= self.steppers[i].need_motor_enable
+        self.need_motor_enable = need_motor_enable
     def query_endstops(self, move_time):
         return homing.QueryEndstops(["x", "y", "z"], self.steppers)
     def check_endstops(self, move):
@@ -94,12 +103,15 @@ class CartKinematics:
                             for i in StepList if axes_d[i]])
         move.limit_speed(velocity_factor * move_d, accel_factor * move_d)
     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]:
                 continue
-            mcu_time, so = self.steppers[i].prep_move(move_time)
+            mcu_stepper = self.steppers[i].mcu_stepper
+            mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             inv_step_dist = self.steppers[i].inv_step_dist
             steps = move.axes_d[i] * inv_step_dist
             move_step_d = move.move_d / abs(steps)
@@ -114,7 +126,7 @@ class CartKinematics:
                 accel_time_offset = move.start_v * inv_accel
                 accel_sqrt_offset = accel_time_offset**2
                 accel_steps = move.accel_r * steps
-                count = so.step_sqrt(
+                count = mcu_stepper.step_sqrt(
                     mcu_time - accel_time_offset, accel_steps, step_offset
                     , accel_sqrt_offset, accel_multiplier)
                 step_pos += count
@@ -125,7 +137,7 @@ class CartKinematics:
                 #t = pos/cruise_v
                 cruise_multiplier = move_step_d * inv_cruise_v
                 cruise_steps = move.cruise_r * steps
-                count = so.step_factor(
+                count = mcu_stepper.step_factor(
                     mcu_time, cruise_steps, step_offset, cruise_multiplier)
                 step_pos += count
                 step_offset += count - cruise_steps
@@ -136,7 +148,7 @@ class CartKinematics:
                 decel_time_offset = move.cruise_v * inv_accel
                 decel_sqrt_offset = decel_time_offset**2
                 decel_steps = move.decel_r * steps
-                count = so.step_sqrt(
+                count = mcu_stepper.step_sqrt(
                     mcu_time + decel_time_offset, decel_steps, step_offset
                     , decel_sqrt_offset, -accel_multiplier)
                 step_pos += count
diff --git a/klippy/delta.py b/klippy/delta.py
index 0fa8caf2..99233cf2 100644
--- a/klippy/delta.py
+++ b/klippy/delta.py
@@ -13,6 +13,7 @@ class DeltaKinematics:
         steppers = ['stepper_a', 'stepper_b', 'stepper_c']
         self.steppers = [stepper.PrinterStepper(printer, config.getsection(n))
                          for n in steppers]
+        self.need_motor_enable = True
         radius = config.getfloat('delta_radius')
         arm_length = config.getfloat('delta_arm_length')
         self.arm_length2 = arm_length**2
@@ -105,6 +106,11 @@ class DeltaKinematics:
         self.limit_xy2 = -1.
         for stepper in self.steppers:
             stepper.motor_enable(move_time, 0)
+        self.need_motor_enable = True
+    def check_motor_enable(self, move_time):
+        for i in StepList:
+            self.steppers[i].motor_enable(move_time, 1)
+        self.need_motor_enable = False
     def query_endstops(self, move_time):
         return homing.QueryEndstops(["a", "b", "c"], self.steppers)
     def check_move(self, move):
@@ -120,6 +126,8 @@ class DeltaKinematics:
     def move_z(self, move_time, move):
         if not move.axes_d[2]:
             return
+        if self.need_motor_enable:
+            self.check_motor_enable(move_time)
         inv_accel = 1. / move.accel
         inv_cruise_v = 1. / move.cruise_v
         for i in StepList:
@@ -128,7 +136,8 @@ class DeltaKinematics:
             tower_d2 = towerx_d**2 + towery_d**2
             height = math.sqrt(self.arm_length2 - tower_d2) + move.start_pos[2]
 
-            mcu_time, so = self.steppers[i].prep_move(move_time)
+            mcu_stepper = self.steppers[i].mcu_stepper
+            mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             inv_step_dist = self.steppers[i].inv_step_dist
             step_dist = self.steppers[i].step_dist
             steps = move.axes_d[2] * inv_step_dist
@@ -143,7 +152,7 @@ class DeltaKinematics:
                 accel_time_offset = move.start_v * inv_accel
                 accel_sqrt_offset = accel_time_offset**2
                 accel_steps = move.accel_r * steps
-                count = so.step_sqrt(
+                count = mcu_stepper.step_sqrt(
                     mcu_time - accel_time_offset, accel_steps, step_offset
                     , accel_sqrt_offset, accel_multiplier)
                 step_pos += count
@@ -154,7 +163,7 @@ class DeltaKinematics:
                 #t = pos/cruise_v
                 cruise_multiplier = step_dist * inv_cruise_v
                 cruise_steps = move.cruise_r * steps
-                count = so.step_factor(
+                count = mcu_stepper.step_factor(
                     mcu_time, cruise_steps, step_offset, cruise_multiplier)
                 step_pos += count
                 step_offset += count - cruise_steps
@@ -165,7 +174,7 @@ class DeltaKinematics:
                 decel_time_offset = move.cruise_v * inv_accel
                 decel_sqrt_offset = decel_time_offset**2
                 decel_steps = move.decel_r * steps
-                count = so.step_sqrt(
+                count = mcu_stepper.step_sqrt(
                     mcu_time + decel_time_offset, decel_steps, step_offset
                     , decel_sqrt_offset, -accel_multiplier)
                 step_pos += count
@@ -175,6 +184,8 @@ class DeltaKinematics:
         if not axes_d[0] and not axes_d[1]:
             self.move_z(move_time, move)
             return
+        if self.need_motor_enable:
+            self.check_motor_enable(move_time)
         move_d = move.move_d
         movez_r = 0.
         inv_movexy_d = 1. / move_d
@@ -243,44 +254,45 @@ class DeltaKinematics:
             step_dist = self.steppers[i].step_dist
             step_pos = self.stepper_pos[i]
             height = step_pos*step_dist - closestz
-            mcu_time, so = self.steppers[i].prep_move(move_time)
+            mcu_stepper = self.steppers[i].mcu_stepper
+            mcu_time = mcu_stepper.print_to_mcu_time(move_time)
             if accel_up_d > 0.:
-                count = so.step_delta_accel(
+                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 = so.step_delta_const(
+                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 = so.step_delta_accel(
+                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 = so.step_delta_accel(
+                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 = so.step_delta_const(
+                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 = so.step_delta_accel(
+                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)
diff --git a/klippy/extruder.py b/klippy/extruder.py
index fb726fca..8b66dad6 100644
--- a/klippy/extruder.py
+++ b/klippy/extruder.py
@@ -11,6 +11,7 @@ class PrinterExtruder:
         self.heater = heater.PrinterHeater(printer, config)
         self.stepper = stepper.PrinterStepper(printer, config)
         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):
@@ -19,6 +20,7 @@ class PrinterExtruder:
         self.stepper.build_config()
     def motor_off(self, move_time):
         self.stepper.motor_enable(move_time, 0)
+        self.need_motor_enable = True
     def check_move(self, move):
         if not self.heater.can_extrude:
             raise homing.EndstopError(move.end_pos, "Extrude below minimum temp")
@@ -28,6 +30,9 @@ class PrinterExtruder:
             # Extrude only move - limit accel and velocity
             move.limit_speed(self.stepper.max_velocity, self.stepper.max_accel)
     def move(self, move_time, move):
+        if self.need_motor_enable:
+            self.stepper.motor_enable(move_time, 1)
+            self.need_motor_enable = False
         axis_d = move.axes_d[3]
         extrude_r = axis_d / move.move_d
         inv_accel = 1. / (move.accel * extrude_r)
@@ -92,7 +97,8 @@ class PrinterExtruder:
         stepper_pos = self.stepper_pos
         inv_step_dist = self.stepper.inv_step_dist
         step_dist = self.stepper.step_dist
-        mcu_time, so = self.stepper.prep_move(move_time)
+        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
 
         # Acceleration steps
@@ -102,7 +108,7 @@ class PrinterExtruder:
             accel_time_offset = start_v * inv_accel
             accel_sqrt_offset = accel_time_offset**2
             accel_steps = accel_d * inv_step_dist
-            count = so.step_sqrt(
+            count = mcu_stepper.step_sqrt(
                 mcu_time - accel_time_offset, accel_steps, step_offset
                 , accel_sqrt_offset, accel_multiplier)
             stepper_pos += count
@@ -113,7 +119,7 @@ class PrinterExtruder:
             #t = pos/cruise_v
             cruise_multiplier = step_dist / cruise_v
             cruise_steps = cruise_d * inv_step_dist
-            count = so.step_factor(
+            count = mcu_stepper.step_factor(
                 mcu_time, cruise_steps, step_offset, cruise_multiplier)
             stepper_pos += count
             step_offset += count - cruise_steps
@@ -124,7 +130,7 @@ class PrinterExtruder:
             decel_time_offset = decel_v * inv_accel
             decel_sqrt_offset = decel_time_offset**2
             decel_steps = decel_d * inv_step_dist
-            count = so.step_sqrt(
+            count = mcu_stepper.step_sqrt(
                 mcu_time + decel_time_offset, decel_steps, step_offset
                 , decel_sqrt_offset, -accel_multiplier)
             stepper_pos += count
@@ -136,7 +142,7 @@ class PrinterExtruder:
             accel_time_offset = retract_v * inv_accel
             accel_sqrt_offset = accel_time_offset**2
             accel_steps = -retract_d * inv_step_dist
-            count = so.step_sqrt(
+            count = mcu_stepper.step_sqrt(
                 mcu_time - accel_time_offset, accel_steps, step_offset
                 , accel_sqrt_offset, accel_multiplier)
             stepper_pos += count
diff --git a/klippy/stepper.py b/klippy/stepper.py
index 8324eebc..51abd093 100644
--- a/klippy/stepper.py
+++ b/klippy/stepper.py
@@ -64,18 +64,14 @@ class PrinterStepper:
         if endstop_pin is not None:
             self.mcu_endstop = mcu.create_endstop(endstop_pin, self.mcu_stepper)
     def motor_enable(self, move_time, enable=0):
+        if enable and self.need_motor_enable:
+            mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
+            self.mcu_stepper.reset_step_clock(mcu_time)
         if (self.mcu_enable is not None
             and self.mcu_enable.get_last_setting() != enable):
             mcu_time = self.mcu_enable.print_to_mcu_time(move_time)
             self.mcu_enable.set_digital(mcu_time, enable)
-        self.need_motor_enable = True
-    def prep_move(self, move_time):
-        mcu_time = self.mcu_stepper.print_to_mcu_time(move_time)
-        if self.need_motor_enable:
-            self.mcu_stepper.reset_step_clock(mcu_time)
-            self.motor_enable(move_time, 1)
-            self.need_motor_enable = False
-        return (mcu_time, self.mcu_stepper)
+        self.need_motor_enable = not enable
     def enable_endstop_checking(self, move_time, step_time):
         mcu_time = self.mcu_endstop.print_to_mcu_time(move_time)
         self.mcu_endstop.home(mcu_time, step_time)
@@ -86,7 +82,7 @@ class PrinterStepper:
         self.mcu_endstop.query_endstop()
         return self.mcu_endstop
     def get_homed_offset(self):
-        if not self.homing_stepper_phases:
+        if not self.homing_stepper_phases or self.need_motor_enable:
             return 0
         pos = self.mcu_endstop.get_last_position()
         pos %= self.homing_stepper_phases