From 52fe770eb2acfd7dbabc069894d7b26c92d615e2 Mon Sep 17 00:00:00 2001
From: Marcio Teixeira <marcio@alephobjects.com>
Date: Tue, 10 Oct 2017 10:34:41 -0600
Subject: [PATCH] Added experimental features stallguard on E0.

---
 Marlin/Conditionals_LulzBot.h | 59 ++++++++++++++++++++++++++++++-----
 Marlin/Configuration_adv.h    |  2 +-
 Marlin/Marlin_main.cpp        |  7 +++--
 3 files changed, 57 insertions(+), 11 deletions(-)

diff --git a/Marlin/Conditionals_LulzBot.h b/Marlin/Conditionals_LulzBot.h
index 96c38e9cb..544c6abc8 100644
--- a/Marlin/Conditionals_LulzBot.h
+++ b/Marlin/Conditionals_LulzBot.h
@@ -1044,21 +1044,64 @@
         else \
             SERIAL_ERRORLNPGM("Probe not triggered");
 
-    /*#define LULZBOT_TMC_G0G1_STALLGUARD_REPORT \
-        static int nextSgReport = 100; \
-        if(planner.blocks_queued()) { \
-            if(nextSgReport-- == 0) { \
-                nextSgReport = 100; \
-                LULZBOT_TMC_REPORT(AXIS)\
+    /* The following function accumulates the average of a
+       stallguard value during a planner move and reports
+       it once the motion ends */
+    #define LULZBOT_TMC_STALLGUARD_AVERAGE(AXIS, SUM, NUM) \
+        static uint8_t   current_tail; \
+        static uint32_t  SUM; \
+        static uint16_t  NUM; \
+        if(current_tail != planner.block_buffer_tail) { \
+            current_tail = planner.block_buffer_tail; \
+            /* When the planner finishes move, report results */ \
+            SERIAL_ECHOLNPAIR(#AXIS " avg_sg: ", SUM/NUM); \
+            SUM = 0; \
+            NUM = 0; \
+        } else if(planner.blocks_queued()) { \
+            /* While in motion, accumulate sg values */ \
+            SUM += stepper##AXIS.DRV_STATUS() & 0b111111111; \
+            NUM++; \
+        }
+
+    /* The following function reports when the average of the
+       stallguard value changes significantly */
+    #define LULZBOT_TMC_STALLGUARD_REPORT_CHANGES(AXIS, SUM, NUM, THRESH) \
+        static uint8_t   current_tail; \
+        static uint32_t  SUM; \
+        static uint16_t  NUM; \
+        static uint16_t  last_avg; \
+        if(current_tail != planner.block_buffer_tail) { \
+            current_tail = planner.block_buffer_tail; \
+            uint16_t avg = SUM/NUM; \
+            if(abs(int(last_avg) - avg) > last_avg * THRESH) { \
+                SERIAL_ECHOLNPAIR("Detected changed in sg value:", avg - last_avg); \
             } \
-        }*/
+            SERIAL_ECHOLNPAIR(#AXIS " avg_sg: ", avg); \
+            last_avg = avg; \
+            SUM = 0; \
+            NUM = 0; \
+        } else if(planner.blocks_queued()) { \
+            /* While in motion, accumulate sg values */ \
+            SUM += stepper##AXIS.DRV_STATUS() & 0b111111111; \
+            NUM++; \
+        }
 
-    #define LULZBOT_TMC_G0G1_STALLGUARD_REPORT
+    #define LULZBOT_TMC_G0G1_STALLGUARD_REPORT \
+        LULZBOT_TMC_STALLGUARD_AVERAGE(E0, sg_sum_e, sg_num_e)
+
+    //#define LULZBOT_TMC_G0G1_STALLGUARD_REPORT
 
     #define LULZBOT_TMC_M119_STALLGUARD_REPORT \
         LULZBOT_TMC_REPORT(X) \
         LULZBOT_TMC_REPORT(Y) \
         LULZBOT_TMC_REPORT(Z)
+
+    #define LULZBOT_TMC2130_ADV { \
+            /* Turn off stealhchop for extruder motor */ \
+            stepperE0.coolstep_min_speed(1024UL * 1024UL - 1UL); \
+            /* Set stallguard value for filament sensing */ \
+            stepperE0.sg_stall_value(5); \
+        }
 #else
     #define LULZBOT_TMC_M119_STALLGUARD_REPORT
     #define LULZBOT_TMC_G0G1_STALLGUARD_REPORT
diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h
index 3dfc9544c..3d674fc27 100644
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@@ -1072,7 +1072,7 @@
    *   stepperX.interpolate(0); \
    * }
    */
-  #define  TMC2130_ADV() {  }
+  #define  TMC2130_ADV() LULZBOT_TMC2130_ADV
 
 #endif // HAVE_TMC2130
 
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 3691ec045..a166df1e8 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -9933,8 +9933,7 @@ inline void gcode_M502() {
 
   static void tmc2130_get_sgt(TMC2130Stepper &st, const char name) {
     SERIAL_CHAR(name);
-    SERIAL_ECHOPGM(" driver homing sensitivity set to ");
-    SERIAL_ECHOLN(st.sgt());
+    SERIAL_ECHOLNPAIR(" driver homing sensitivity set to ", st.sgt());
   }
   static void tmc2130_set_sgt(TMC2130Stepper &st, const char name, const int8_t sgt_val) {
     st.sgt(sgt_val);
@@ -10057,6 +10056,10 @@ inline void gcode_M502() {
         if (parser.seen(axis_codes[Y_AXIS])) tmc2130_set_sgt(stepperY, 'Y', parser.value_int());
         else tmc2130_get_sgt(stepperY, 'Y');
       #endif
+      #if ENABLED(E0_IS_TMC2130)
+        if (parser.seen(axis_codes[E_AXIS])) tmc2130_set_sgt(stepperE0, 'E', parser.value_int());
+        else tmc2130_get_sgt(stepperE0, 'E');
+      #endif
     }
   #endif // SENSORLESS_HOMING