From ccfd5c1010270493f0c7ddc7a5ab62dc5e09def3 Mon Sep 17 00:00:00 2001
From: Fabio Santos <fabiosan@live.com>
Date: Mon, 20 Apr 2020 06:08:00 -0700
Subject: [PATCH] Trinamic steppers Homing Phase (#17299)

---
 Marlin/Configuration_adv.h    | 18 +++++--
 Marlin/src/feature/tmc_util.h |  6 ++-
 Marlin/src/module/motion.cpp  | 88 +++++++++++++++++++++++++++++++++--
 3 files changed, 104 insertions(+), 8 deletions(-)

diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 05c629d8530..510a8536ac1 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -2272,9 +2272,9 @@
   #define CHOPPER_TIMING CHOPPER_DEFAULT_12V
 
   /**
-   * Monitor Trinamic drivers for error conditions,
-   * like overtemperature and short to ground.
-   * In the case of overtemperature Marlin can decrease the driver current until error condition clears.
+   * Monitor Trinamic drivers
+   * for error conditions like overtemperature and short to ground.
+   * To manage over-temp Marlin can decrease the driver current until the error condition clears.
    * Other detected conditions can be used to stop the current print.
    * Relevant g-codes:
    * M906 - Set or get motor current in milliamps using axis codes X, Y, Z, E. Report values if no axis codes given.
@@ -2351,6 +2351,18 @@
     //#define IMPROVE_HOMING_RELIABILITY
   #endif
 
+  /**
+   * TMC Homing stepper phase.
+   *
+   * Improve homing repeatability by homing to stepper coil's nearest absolute
+   * phase position. Trinamic drivers use a stepper phase table with 1024 values
+   * spanning 4 full steps with 256 positions each (ergo, 1024 positions).
+   * Full step positions (128, 384, 640, 896) have the highest holding torque.
+   *  
+   * Values from 0..1023, -1 to disable homing phase for that axis.
+   */
+   //#define TMC_HOME_PHASE { 896, 896, 896 }
+
   /**
    * Beta feature!
    * Create a 50/50 square wave step pulse optimal for stepper drivers.
diff --git a/Marlin/src/feature/tmc_util.h b/Marlin/src/feature/tmc_util.h
index ccae8b660ca..6838aa433c9 100644
--- a/Marlin/src/feature/tmc_util.h
+++ b/Marlin/src/feature/tmc_util.h
@@ -105,7 +105,8 @@ class TMCMarlin : public TMC, public TMCStorage<AXIS_LETTER, DRIVER_ID> {
       this->val_mA = mA;
       TMC::rms_current(mA, mult);
     }
-
+    inline uint16_t get_microstep_counter() { return TMC::MSCNT(); }
+ 
     #if HAS_STEALTHCHOP
       inline void refresh_stepping_mode() { this->en_pwm_mode(this->stored.stealthChop_enabled); }
       inline bool get_stealthChop_status() { return this->en_pwm_mode(); }
@@ -170,6 +171,7 @@ class TMCMarlin<TMC2208Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC220
       this->val_mA = mA;
       TMC2208Stepper::rms_current(mA, mult);
     }
+    inline uint16_t get_microstep_counter() { return TMC2208Stepper::MSCNT(); }
 
     #if HAS_STEALTHCHOP
       inline void refresh_stepping_mode() { en_spreadCycle(!this->stored.stealthChop_enabled); }
@@ -216,6 +218,7 @@ class TMCMarlin<TMC2209Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC220
       this->val_mA = mA;
       TMC2209Stepper::rms_current(mA, mult);
     }
+    inline uint16_t get_microstep_counter() { return TMC2209Stepper::MSCNT(); }
 
     #if HAS_STEALTHCHOP
       inline void refresh_stepping_mode() { en_spreadCycle(!this->stored.stealthChop_enabled); }
@@ -273,6 +276,7 @@ class TMCMarlin<TMC2660Stepper, AXIS_LETTER, DRIVER_ID, AXIS_ID> : public TMC266
       this->val_mA = mA;
       TMC2660Stepper::rms_current(mA);
     }
+    inline uint16_t get_microstep_counter() { return TMC2660Stepper::mstep(); }
 
     #if USE_SENSORLESS
       inline int16_t homing_threshold() { return TMC2660Stepper::sgt(); }
diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp
index 3c89b933d8c..c2301aa3435 100644
--- a/Marlin/src/module/motion.cpp
+++ b/Marlin/src/module/motion.cpp
@@ -1483,6 +1483,80 @@ void set_axis_not_trusted(const AxisEnum axis) {
   #endif
 }
 
+/**
+ * Move the axis back to its home_phase if set and driver is capable (TMC)
+ *
+ * Improves homing repeatability by homing to stepper coil's nearest absolute
+ * phase position. Trinamic drivers use a stepper phase table with 1024 values
+ * spanning 4 full steps with 256 positions each (ergo, 1024 positions).
+ */
+void backout_to_tmc_homing_phase(const AxisEnum axis) {
+  #ifdef TMC_HOME_PHASE
+    const abc_long_t home_phase = TMC_HOME_PHASE;
+
+    // check if home phase is disabled for this axis.
+    if (home_phase[axis] < 0) return;
+
+    int16_t axisMicrostepSize;
+    int16_t phaseCurrent;
+    bool invertDir;
+
+    switch (axis) {
+      #ifdef X_MICROSTEPS
+        case X_AXIS:
+          axisMicrostepSize = 256 / (X_MICROSTEPS);
+          phaseCurrent = stepperX.get_microstep_counter();
+          invertDir = INVERT_X_DIR;
+          break;
+      #endif
+      #ifdef Y_MICROSTEPS
+        case Y_AXIS:
+          axisMicrostepSize = 256 / (Y_MICROSTEPS);
+          phaseCurrent = stepperY.get_microstep_counter();
+          invertDir = INVERT_Y_DIR;
+          break;
+      #endif
+      #ifdef Z_MICROSTEPS
+        case Z_AXIS:
+          axisMicrostepSize = 256 / (Z_MICROSTEPS);
+          phaseCurrent = stepperZ.get_microstep_counter();
+          invertDir = INVERT_Z_DIR;
+          break;
+      #endif
+      default: return;
+    }
+
+    // Depending on invert dir measure the distance to nearest home phase.
+    int16_t phaseDelta = (invertDir ? -1 : 1) * (home_phase[axis] - phaseCurrent);
+
+    // Check if home distance within endstop assumed repeatability noise of .05mm and warn.
+    if (ABS(phaseDelta) * planner.steps_to_mm[axis] / axisMicrostepSize < 0.05f)
+      DEBUG_ECHOLNPAIR("Selected home phase ", home_phase[axis],
+                       " too close to endstop trigger phase ", phaseCurrent,
+                       ". Pick a different phase for ", axis_codes[axis]);
+
+    // Skip to next if target position is behind current. So it only moves away from endstop.
+    if (phaseDelta < 0) phaseDelta += 1024;
+
+    // Get the integer µsteps to target. Unreachable phase? Consistently stop at the µstep before / after based on invertDir.
+    const float mmDelta = -(int16_t(phaseDelta / axisMicrostepSize) * planner.steps_to_mm[axis] * (Z_HOME_DIR));
+
+    // optional debug messages.
+    if (DEBUGGING(LEVELING)) {
+      DEBUG_ECHOLNPAIR(
+        "Endstop ", axis_codes[axis], " hit at Phase:", phaseCurrent,
+        " Delta:", phaseDelta, " Distance:", mmDelta
+      );
+    }
+
+    if (mmDelta != 0) {
+      // retrace by the amount computed in mmDelta.
+      do_homing_move(axis, mmDelta, get_homing_bump_feedrate(axis));
+    }
+  #endif
+}
+
+
 /**
  * Home an individual "raw axis" to its endstop.
  * This applies to XYZ on Cartesian and Core robots, and
@@ -1742,6 +1816,9 @@ void homeaxis(const AxisEnum axis) {
     }
   #endif
 
+  // move back to homing phase if configured and capable
+  backout_to_tmc_homing_phase(axis);
+
   #if IS_SCARA
 
     set_axis_is_at_home(axis);
@@ -1753,10 +1830,13 @@ void homeaxis(const AxisEnum axis) {
     // so here it re-homes each tower in turn.
     // Delta homing treats the axes as normal linear axes.
 
-    // retrace by the amount specified in delta_endstop_adj + additional dist in order to have minimum steps
-    if (delta_endstop_adj[axis] * Z_HOME_DIR <= 0) {
-      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("delta_endstop_adj:");
-      do_homing_move(axis, delta_endstop_adj[axis] - (MIN_STEPS_PER_SEGMENT + 1) * planner.steps_to_mm[axis] * Z_HOME_DIR);
+    const float adjDistance = delta_endstop_adj[axis],
+                minDistance = (MIN_STEPS_PER_SEGMENT) * planner.steps_to_mm[axis];
+
+    // Retrace by the amount specified in delta_endstop_adj if more than min steps.
+    if (adjDistance * (Z_HOME_DIR) < 0 && ABS(adjDistance) > minDistance) { // away from endstop, more than min distance
+      if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("adjDistance:", adjDistance);
+      do_homing_move(axis, adjDistance, get_homing_bump_feedrate(axis));
     }
 
   #else // CARTESIAN / CORE