From 0d30ccf7671629eb79f1038bd5aae68ac2d56a91 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Fri, 11 Jan 2019 18:08:05 -0600
Subject: [PATCH] Tweaks to TMC26XStepper

---
 Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp | 63 ++++++++------------------
 Marlin/src/HAL/HAL_STM32F7/TMC2660.h   |  4 +-
 2 files changed, 22 insertions(+), 45 deletions(-)

diff --git a/Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp b/Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp
index e01a0b42e..e4fa63c8a 100644
--- a/Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp
+++ b/Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp
@@ -127,7 +127,7 @@ uint8_t current_scaling = 0;
  * dir_pin - the pin where the direction pin is connected
  * step_pin - the pin where the step pin is connected
  */
-TMC26XStepper::TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor) {
+TMC26XStepper::TMC26XStepper(const int16_t in_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor) {
   // We are not started yet
   started = false;
 
@@ -165,7 +165,7 @@ TMC26XStepper::TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t di
   // Set a nice microstepping value
   setMicrosteps(DEFAULT_MICROSTEPPING_VALUE);
   // Save the number of steps
-  this->number_of_steps = number_of_steps;
+  number_of_steps = in_steps;
 }
 
 
@@ -389,52 +389,29 @@ char TMC26XStepper::getStallGuardFilter(void) {
  * any value in between will be mapped to the next smaller value
  * 0 and 1 set the motor in full step mode
  */
-void TMC26XStepper::setMicrosteps(int16_t number_of_steps) {
-  long setting_pattern;
-  //poor mans log
-  if (number_of_steps >= 256) {
-    setting_pattern = 0;
-    microsteps = 256;
-  }
-  else if (number_of_steps >= 128) {
-    setting_pattern = 1;
-    microsteps = 128;
-  }
-  else if (number_of_steps >= 64) {
-    setting_pattern = 2;
-    microsteps = 64;
-  }
-  else if (number_of_steps >= 32) {
-    setting_pattern = 3;
-    microsteps = 32;
-  }
-  else if (number_of_steps >= 16) {
-    setting_pattern = 4;
-    microsteps = 16;
-  }
-  else if (number_of_steps >= 8) {
-    setting_pattern = 5;
-    microsteps = 8;
-  }
-  else if (number_of_steps >= 4) {
-    setting_pattern = 6;
-    microsteps = 4;
-  }
-  else if (number_of_steps >= 2) {
-    setting_pattern = 7;
-    microsteps = 2;
-    //1 and 0 lead to full step
-  }
-  else if (number_of_steps <= 1) {
-    setting_pattern = 8;
-    microsteps = 1;
-  }
+void TMC26XStepper::setMicrosteps(const int16_t in_steps) {
+  uint16_t setting_pattern;
+
+       if (in_steps >= 256) setting_pattern = 0;
+  else if (in_steps >= 128) setting_pattern = 1;
+  else if (in_steps >=  64) setting_pattern = 2;
+  else if (in_steps >=  32) setting_pattern = 3;
+  else if (in_steps >=  16) setting_pattern = 4;
+  else if (in_steps >=   8) setting_pattern = 5;
+  else if (in_steps >=   4) setting_pattern = 6;
+  else if (in_steps >=   2) setting_pattern = 7;
+  else if (in_steps <=   1) setting_pattern = 8; // 1 and 0 lead to full step
+
+  microsteps = _BV(8 - setting_pattern);
+
   #ifdef TMC_DEBUG0 // crashes
     //SERIAL_PRINTF("Microstepping: ");
     SERIAL_ECHOPAIR("\n Microstepping: ", microsteps);
   #endif
+
   // Delete the old value
-  this->driver_control_register_value &= 0xFFFF0UL;
+  this->driver_control_register_value &= 0x000FFFF0UL;
+
   // Set the new value
   this->driver_control_register_value |= setting_pattern;
 
diff --git a/Marlin/src/HAL/HAL_STM32F7/TMC2660.h b/Marlin/src/HAL/HAL_STM32F7/TMC2660.h
index 9a26b3204..e9b189e14 100644
--- a/Marlin/src/HAL/HAL_STM32F7/TMC2660.h
+++ b/Marlin/src/HAL/HAL_STM32F7/TMC2660.h
@@ -119,7 +119,7 @@ class TMC26XStepper {
      * You can select a different stepping with setMicrosteps() to aa different value.
      * \sa start(), setMicrosteps()
      */
-    TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor=100); //resistor=150
+    TMC26XStepper(const int16_t in_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor=100); //resistor=150
 
     /*!
      * \brief configures and starts the TMC26X stepper driver. Before you called this function the stepper driver is in nonfunctional mode.
@@ -163,7 +163,7 @@ class TMC26XStepper {
      * If you give any other value it will be rounded to the next smaller number (3 would give a microstepping of 2).
      * You can always check the current microstepping with getMicrosteps().
      */
-    void setMicrosteps(int16_t number_of_steps);
+    void setMicrosteps(const int16_t in_steps);
 
     /*!
      * \brief returns the effective current number of microsteps selected.