diff --git a/tmc2209.c b/tmc2209.c
index 03afe03..d6e1421 100644
--- a/tmc2209.c
+++ b/tmc2209.c
@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <string.h>
 
 #include "tmc2209.h"
+#include "grbl/hal.h"
 
 
 static const TMC2209_t tmc2209_defaults = {
@@ -178,6 +179,10 @@ void TMC2209_SetCurrent (TMC2209_t *driver, uint16_t mA, uint8_t hold_pct)
 
     _set_rms_current(driver);
 
+    hal.stream.write("[Set current: ");
+    hal.stream.write(uitoa(driver->config.current ));
+    hal.stream.write("]" ASCII_EOL);
+
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->chopconf);
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->ihold_irun);
 }
@@ -190,12 +195,19 @@ float TMC2209_GetTPWMTHRS (TMC2209_t *driver, float steps_mm)
 void TMC2209_SetTPWMTHRS (TMC2209_t *driver, float mm_sec, float steps_mm)
 {
     driver->tpwmthrs.reg.tpwmthrs = tmc_calc_tstep(&driver->config, mm_sec, steps_mm);
+
+    hal.stream.write("[SG: TPWMTHRS rate set: ");
+    hal.stream.write(uitoa(driver->tpwmthrs.reg.tpwmthrs ));
+    hal.stream.write("]" ASCII_EOL);
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->tpwmthrs);
 }
 
 void TMC2209_SetTCOOLTHRS (TMC2209_t *driver, float mm_sec, float steps_mm) // -> pwm threshold
 {
     driver->tcoolthrs.reg.tcoolthrs = tmc_calc_tstep(&driver->config, mm_sec, steps_mm);
+    hal.stream.write("[SG: TCOOLTHRS set: ");
+    hal.stream.write(uitoa(driver->tcoolthrs.reg.tcoolthrs ));
+    hal.stream.write("]" ASCII_EOL);
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->tcoolthrs);
 }
 
diff --git a/tmc2209.h b/tmc2209.h
index 2456955..de8f904 100644
--- a/tmc2209.h
+++ b/tmc2209.h
@@ -95,11 +95,11 @@ typedef enum {
 #define TMC2209_COOLSTEP_THRS       TMC_THRESHOLD_MIN   // tpwmthrs: 0 - 2^20 - 1 (20 bits)
 
 // COOLCONF - CoolStep defaults
-#define TMC2209_SEMIN               5   // 0 = coolStep off, 1 - 15 = coolStep on
+#define TMC2209_SEMIN               7   // 0 = coolStep off, 1 - 15 = coolStep on // Basic config: 1 +  1/16 * SG_RESULT
 #define TMC2209_SEUP                0   // 0 - 3 (1 - 8)
 #define TMC2209_SEMAX               2   // 0 - 15
 #define TMC2209_SEDN                1   // 0 - 3
-#define TMC2209_SEIMIN              0   // boolean (0 or 1)
+#define TMC2209_SEIMIN              0   // boolean (0 or 1) (1/2 or 1/4 of currents setting IRUN)
 
 // end of default values
 
diff --git a/tmc2209hal.c b/tmc2209hal.c
index 50eca33..8546296 100644
--- a/tmc2209hal.c
+++ b/tmc2209hal.c
@@ -149,23 +149,40 @@ static void setTCoolThrs (uint8_t motor, float mm_sec, float steps_mm)
 static void setTCoolThrsRaw (uint8_t motor, uint32_t value)
 {
     tmcdriver[motor]->tcoolthrs.reg.tcoolthrs = value;
+    hal.stream.write("[setTCoolThrsRaw:  ");
+    hal.stream.write(uitoa(value));
+    hal.stream.write("]" ASCII_EOL);
     TMC2209_WriteRegister(tmcdriver[motor], (TMC2209_datagram_t *)&tmcdriver[motor]->tcoolthrs);
 }
 
 static void stallGuardEnable (uint8_t motor, float feed_rate, float steps_mm, int16_t sensitivity)
 {
+    hal.stream.write("[SG: Motor: ");
+    hal.stream.write(uitoa(motor));
+    hal.stream.write("]" ASCII_EOL);
     TMC2209_t *driver = tmcdriver[motor];
 
     driver->gconf.reg.en_spreadcycle = false; // stealthChop on
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->gconf);
 
-    driver->pwmconf.reg.pwm_autoscale = false;
+    driver->pwmconf.reg.pwm_autoscale = true;
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->pwmconf);
 
     TMC2209_SetTCOOLTHRS(driver, feed_rate / (60.0f * 1.5f), steps_mm);
 
+    // sprintf(sbuf, "%-15s", "Peak current");
+    //     for(motor = 0; motor < n_motors; motor++) {
+    //         if(bit_istrue(axes, bit(motor_map[motor].axis)))
+    //             sprintf(append(sbuf), "%8" UINT32SFMT, (uint32_t)((float)debug_report[motor].current * sqrtf(2)));
+    //     }
+    // write_line(sbuf);
+
     driver->sgthrs.reg.threshold = (uint8_t)sensitivity;
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->sgthrs);
+
+    hal.stream.write("[SG: Sens set: ");
+    hal.stream.write(uitoa(sensitivity));
+    hal.stream.write("]" ASCII_EOL);
 }
 
 static void stealthChopEnable (uint8_t motor)
@@ -179,6 +196,8 @@ static void stealthChopEnable (uint8_t motor)
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->pwmconf);
 
     setTCoolThrsRaw(motor, 0);
+
+    hal.stream.write("[Enabled stealthchop: " ASCII_EOL);
 }
 
 static void coolStepEnable (uint8_t motor)
@@ -192,6 +211,8 @@ static void coolStepEnable (uint8_t motor)
     TMC2209_WriteRegister(driver, (TMC2209_datagram_t *)&driver->pwmconf);
 
     setTCoolThrsRaw(motor, 0);
+    hal.stream.write("[Enabled coolstep: " ASCII_EOL);
+
 }
 
 static float getTPWMThrs (uint8_t motor, float steps_mm)