From 4d6bd3492d85847d9ef8b8b612502c5b9245c513 Mon Sep 17 00:00:00 2001
From: bdring <barton.dring@gmail.com>
Date: Sun, 2 Jan 2022 13:44:19 -0600
Subject: [PATCH] Update TrinamicDriver.cpp

---
 Grbl_Esp32/src/Motors/TrinamicDriver.cpp | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
index 84e6ce2a..a96dd63c 100644
--- a/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
+++ b/Grbl_Esp32/src/Motors/TrinamicDriver.cpp
@@ -376,16 +376,25 @@ namespace Motors {
 
         digitalWrite(_disable_pin, _disabled);
 
+        uint8_t _toff_value = 0;
+
 #ifdef USE_TRINAMIC_ENABLE
         if (_disabled) {
-            tmcstepper->toff(TRINAMIC_TOFF_DISABLE);
+            _toff_value = TRINAMIC_TOFF_DISABLE;
         } else {
             if (_mode == TrinamicMode::StealthChop) {
-                tmcstepper->toff(TRINAMIC_TOFF_STEALTHCHOP);
+                _toff_value = TRINAMIC_TOFF_STEALTHCHOP;
             } else {
-                tmcstepper->toff(TRINAMIC_TOFF_COOLSTEP);
+                _toff_value = TRINAMIC_TOFF_COOLSTEP;
             }
         }
+
+        if (tmc2130) {
+            tmc2130->toff(_toff_value);
+        } else {
+            tmc5160->toff(_toff_value);
+        }
+
 #endif
         // the pin based enable could be added here.
         // This would be for individual motors, not the single pin for all motors.