Add Cartesian Correction

Add Hysteresis correction
Add Zwobble correction

Documentation/GCodes.md

@@ -52,6 +52,11 @@
 *  M84  - Disable steppers until next move, or use S[seconds] to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
 *  M85  - Set inactivity shutdown timer with parameter S[seconds]. To disable set zero (default)
 *  M92  - Set axis_steps_per_unit - same syntax as G92
+*  M96  - Print ZWobble value
+*  M97  - Set ZWobble parameter M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees>
+*  M98  - Print Hysteresis value
+*  M99  - Set Hysteresis parameter M99 X<in mm> Y<in mm> Z<in mm> E<in mm>
+*  M100 - Watch Free Memory (For Debugging Only)
 *  M104 - Set extruder target temp
 *  M105 - Read current temp
 *  M106 - Fan on

Documentation/changelog.md

 ### Version 4.2.7
 * Add M906 Set motor Currents for ALLIGATOR board
 * Add M408 JSON OUTPUT
+* Add Cartesian Correction Hysteresis and Zwooble
+* Bug fix
 
 ### Version 4.2.6
 * Bug Fix

MK/Configuration_Cartesian.h

@@ -24,6 +24,7 @@
  * - Axis accelleration
  * - Homing feedrate
  * - Hotend offset
+ * - Cartesian Correction
  *
  * Basic-settings can be found in Configuration_Basic.h
  * Feature-settings can be found in Configuration_Feature.h
@@ -423,4 +424,37 @@
 //#define HOTEND_OFFSET_Z {0.0, 0.0, 0.0, 0.0} // (in mm) for each hotend, offset of the hotend on the Z axis
 /*****************************************************************************************/
 
+
+/*****************************************************************************************
+ ******************************** CARTESIAN CORRECTION ***********************************
+ *****************************************************************************************
+ *                                                                                       *
+ * New functions, Hysteresis and Zwobble.                                                *
+ *                                                                                       *
+ * Hysteresis:                                                                           *
+ * These are the extra distances that are performed when an axis changes direction       *
+ * to compensate for any mechanical hysteresis your printer has.                         *
+ * Set the parameters width M99 X<in mm> Y<in mm> Z<in mm> E<in mm>                      *
+ *                                                                                       *
+ * ZWobble:                                                                              *
+ * How to use it:                                                                        *
+ * Set the parameters with M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees>    *
+ * KNOWN LIMITATION (by design): if you redefine the Z value during your print           *
+ * (with a G92 for example), the correction *will* screw up                              *
+ * How does it work?                                                                     *
+ * This class compensates for a wobble of the Z axis that makes the translation          *
+ * rod movement->bed (extruder) movement nonlinear.                                      *
+ * Instead of assuming Zactual = Zrod, the function assumes that                         *
+ * Zaxtual = Zrod + A*sin(w*Zrod + phase). Since the user wants to specify Zactual,      *
+ * we need to invert the formula to obtain Zrod, which is the value that will serve      *
+ * as the input of the motor.                                                            *
+ *                                                                                       *
+ *****************************************************************************************/
+//define HYSTERESIS
+//define ZWOBBLE
+
+#define DEFAULT_HYSTERESIS_MM   0, 0, 0, 0  // X, Y, Z, E hysteresis in mm.
+#define DEFAULT_ZWOBBLE         0, 0, 0     // A, W, P
+/*****************************************************************************************/
+
 #endif
\ No newline at end of file

MK/MK.ino

@@ -90,6 +90,10 @@
  *        or use S<seconds> to specify an inactivity timeout, after which the steppers will be disabled.  S0 to disable the timeout.
  * M85  - Set inactivity shutdown timer with parameter S<seconds>. To disable set zero (default)
  * M92  - Set axis_steps_per_unit - same syntax as G92
+ * M96  - Print ZWobble value
+ * M97  - Set ZWobble parameter M97 A<Amplitude_in_mm> W<period_in_mm> P<phase_in_degrees>
+ * M98  - Print Hysteresis value
+ * M99  - Set Hysteresis parameter M99 X<in mm> Y<in mm> Z<in mm> E<in mm>
  * M100 - Watch Free Memory (For Debugging Only)
  * M104 - Set extruder target temp
  * M105 - Read current temp

MK/base.h

@@ -46,39 +46,23 @@
 #include "module/motion/stepper.h"
 #include "module/motion/stepper_indirection.h"
 #include "module/motion/planner.h"
+#include "module/motion/vector_3.h"
+#include "module/motion/qr_solve.h"
+#include "module/motion/cartesian_correction.h"
 #include "module/temperature/temperature.h"
 #include "module/temperature/thermistortables.h"
 #include "module/lcd/ultralcd.h"
+#include "module/lcd/buzzer.h"
 #include "module/nextion/Nextion_lcd.h"
 #include "module/sd/cardreader.h"
-
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-  #include "module/motion/vector_3.h"
-  #if ENABLED(AUTO_BED_LEVELING_GRID)
-    #include "module/motion/qr_solve.h"
-  #endif
-#endif
+#include "module/servo/servo.h"
+#include "module/watchdog/watchdog.h"
+#include "module/blinkm/blinkm.h"
 
 #if MB(ALLIGATOR)
   #include "module/alligator/external_dac.h"
 #endif
 
-#if ENABLED(USE_WATCHDOG)
-  #include "module/watchdog/watchdog.h"
-#endif
-
-#if HAS(BUZZER)
-  #include "module/lcd/buzzer.h"
-#endif
-
-#if ENABLED(BLINKM)
-  #include "module/blinkm/blinkm.h"
-#endif
-
-#if HAS(SERVOS)
-  #include "module/servo/servo.h"
-#endif
-
 #if HAS(DIGIPOTSS)
   #include <SPI.h>
 #endif
@@ -87,4 +71,8 @@
   #include "module/fwtest/firmware_test.h"
 #endif
 
+#if ENABLED(RFID_MODULE)
+  #include "module/mfrc522/MFRC522_serial.h"
+#endif
+
 #endif

MK/module/HAL/fastio.h

@@ -6,6 +6,7 @@
 #ifndef _FASTIO_ARDUINO_H
 #define _FASTIO_ARDUINO_H
 
+#include <avr/io.h>
 
 /*
   utility functions

MK/module/MK_Main.cpp

@@ -4843,6 +4843,51 @@ inline void gcode_M92() {
   plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
 }
 
+#if ENABLED(ZWOBBLE)
+  /**
+   * M96: Print ZWobble value
+   */
+  inline void gcode_M96() {
+    zwobble.ReportToSerial();
+  }
+
+  /**
+   * M97: Set ZWobble value
+   */
+  inline void gcode_M97() {
+    float zVal = -1, hVal = -1, lVal = -1;
+
+    if (code_seen('A')) zwobble.setAmplitude(code_value());
+    if (code_seen('W')) zwobble.setPeriod(code_value());
+    if (code_seen('P')) zwobble.setPhase(code_value());
+    if (code_seen('Z')) zVal = code_value();
+    if (code_seen('H')) hVal = code_value();
+    if (code_seen('L')) lVal = code_value();
+    if (zVal >= 0 && hVal >= 0) zwobble.setSample(zVal, hVal);
+    if (zVal >= 0 && lVal >= 0) zwobble.setScaledSample(zVal, lVal);
+    if (lVal >  0 && hVal >  0) zwobble.setScalingFactor(hVal/lVal);
+  }
+#endif // ZWOBBLE
+
+#if ENABLED(HYSTERESIS)
+  /**
+   * M98: Print Hysteresis value
+   */
+  inline void gcode_M98() {
+    hysteresis.ReportToSerial();
+  }
+
+  /**
+   * M99: Set Hysteresis value
+   */
+  inline void gcode_M99() {
+    for(uint8_t i = 0; i < NUM_AXIS; i++) {
+      if (code_seen(axis_codes[i]))
+        hysteresis.SetAxis(i, code_value());
+    }
+  }
+#endif // HYSTERESIS
+
 /**
  * M100 Free Memory Watcher
  *
@@ -6470,7 +6515,7 @@ inline void gcode_M428() {
       sync_plan_position();
     #endif
     ECHO_LM(DB, "Offset applied.");
-    LCD_ALERTMESSAGEPGM("Offset applied.");
+    LCD_MESSAGEPGM("Offset applied.");
     #if HAS(BUZZER)
       enqueuecommands_P(PSTR("M300 S659 P200\nM300 S698 P200"));
     #endif
@@ -7455,6 +7500,26 @@ void process_next_command() {
         gcode_M85(); break;
       case 92: // M92 Set the steps-per-unit for one or more axes
         gcode_M92(); break;
+
+      #if ENABLED(ZWOBBLE)
+        case 96: // M96 Print ZWobble value
+          gcode_M96(); break;
+        case 97: // M97 Set ZWobble parameter
+          gcode_M97(); break;
+      #endif
+
+      #if ENABLED(HYSTERESIS)
+        case 98: // M98 Print Hysteresis value
+          gcode_M98(); break;
+        case 99: // M99 Set Hysteresis parameter
+          gcode_M99(); break;
+      #endif
+
+      #if ENABLED(M100_FREE_MEMORY_WATCHER)
+        case 100:
+          gcode_M100(); break;
+      #endif
+
       case 104: // M104
         gcode_M104(); break;
       case 105: // M105 Read current temperature
@@ -7469,12 +7534,6 @@ void process_next_command() {
 
       case 109: // M109 Wait for temperature
         gcode_M109(); break;
-
-      #if ENABLED(M100_FREE_MEMORY_WATCHER)
-        case 100:
-          gcode_M100(); break;
-      #endif
-
       case 110: break; // M110: Set line number - don't show "unknown command"
       case 111: // M111 Set debug level
         gcode_M111(); break;

MK/module/blinkm/blinkm.cpp

-/*
-  blinkm.cpp - Library for controlling a BlinkM over i2c
-  Created by Tim Koster, August 21 2013.
-*/
+/**
+ * blinkm.cpp - Library for controlling a BlinkM over i2c
+ * Created by Tim Koster, August 21 2013.
+ */
+
 #include "../../base.h"
 
 #if ENABLED(BLINKM)
+  #include "blinkm.h"
+  #include <Wire.h>
 
-#include "blinkm.h"
-#include <Wire.h>
-
-void SendColors(byte red, byte grn, byte blu) {
+  void SendColors(byte red, byte grn, byte blu) {
     Wire.begin();
     Wire.beginTransmission(0x09);
     Wire.write('o');                    //to disable ongoing script, only needs to be used once
@@ -18,7 +18,6 @@ void SendColors(byte red, byte grn, byte blu) {
     Wire.write(grn);
     Wire.write(blu);
     Wire.endTransmission();
-}
+  }
 
 #endif // BLINKM
-

MK/module/blinkm/blinkm.h

@@ -4,8 +4,10 @@
  */
 
 #ifndef BLINKM_H
-#define BLINKM_H
+  #define BLINKM_H
 
-void SendColors(byte red, byte grn, byte blu);
+  #if ENABLED(BLINKM)
+    void SendColors(byte red, byte grn, byte blu);
+  #endif
 
 #endif

MK/module/communication/communication.h

@@ -6,6 +6,7 @@
   #define ER          "Error: "             // error for host
   #define WT          "Wait"                // wait for host
   #define DB          "Echo: "              // message for user
+  #define DEB         "Debug: "             // message for debug
   #define CFG         "Config: "            // config for host
   #define INFO        "Info: "              // info for host
   #define RESEND      "Resend: "            // resend for host

MK/module/lcd/ultralcd_st7920_u8glib_rrd.h

@@ -35,8 +35,8 @@ static void ST7920_SWSPI_SND_8BIT(uint8_t val) {
   }
 }
 
-#define ST7920_CS()              {WRITE(ST7920_CS_PIN,1);u8g_10MicroDelay();}
-#define ST7920_NCS()             {WRITE(ST7920_CS_PIN,0);}
+#define ST7920_CS()              {WRITE(ST7920_CS_PIN, 1); u8g_10MicroDelay();}
+#define ST7920_NCS()             {WRITE(ST7920_CS_PIN, 0);}
 #define ST7920_SET_CMD()         {ST7920_SWSPI_SND_8BIT(0xf8);u8g_10MicroDelay();}
 #define ST7920_SET_DAT()         {ST7920_SWSPI_SND_8BIT(0xfa);u8g_10MicroDelay();}
 #define ST7920_WRITE_BYTE(a)     {ST7920_SWSPI_SND_8BIT((uint8_t)((a)&0xf0u));ST7920_SWSPI_SND_8BIT((uint8_t)((a)<<4u));u8g_10MicroDelay();}

MK/module/motion/cartesian_correction.cpp

+/**
+ * cartesian_correction.cpp
+ * A class that manages hysteresis by inserting extra plan_buffer_line when necessary
+ * A class that manages ZWobble
+ *
+ * Copyright (c) 2016 MagoKimbra
+ * Copyright (c) 2013 Francesco Santini
+ * Copyright (c) 2012 Neil James Martin
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "../../base.h"
+#include "cartesian_correction.h"
+
+#ifdef HYSTERESIS
+  //===========================================================================
+  Hysteresis hysteresis(DEFAULT_HYSTERESIS_MM);
+  float axis_shift[NUM_AXIS] = { 0.0f, 0.0f, 0.0f, 0.0f };
+
+  //===========================================================================
+  Hysteresis::Hysteresis(float x_mm, float y_mm, float z_mm, float e_mm) {
+    m_prev_direction_bits = 0;
+    Set(x_mm, y_mm, z_mm, e_mm);
+  }
+
+  //===========================================================================
+  void Hysteresis::Set(float x_mm, float y_mm, float z_mm, float e_mm) {
+    m_hysteresis_mm[X_AXIS] = x_mm;
+    m_hysteresis_mm[Y_AXIS] = y_mm;
+    m_hysteresis_mm[Z_AXIS] = z_mm;
+    m_hysteresis_mm[E_AXIS] = e_mm;
+
+    m_hysteresis_bits = ((m_hysteresis_mm[X_AXIS] != 0.0f) ? (1 << X_AXIS) : 0)
+                      | ((m_hysteresis_mm[Y_AXIS] != 0.0f) ? (1 << Y_AXIS) : 0)
+                      | ((m_hysteresis_mm[Z_AXIS] != 0.0f) ? (1 << Z_AXIS) : 0)
+                      | ((m_hysteresis_mm[E_AXIS] != 0.0f) ? (1 << E_AXIS) : 0);
+
+    calcSteps();
+  }
+
+  //===========================================================================
+  void Hysteresis::SetAxis(uint8_t axis, float mm) {
+    m_hysteresis_mm[axis] = mm;
+
+    if(mm != 0.0f)  m_hysteresis_bits |=  ( 1 << axis);
+    else            m_hysteresis_bits &= ~( 1 << axis);
+
+    calcSteps();
+    ReportToSerial();
+  }
+
+  //===========================================================================
+  void Hysteresis::calcSteps() {
+    for (uint8_t i = 0; i < NUM_AXIS; i++)
+      m_hysteresis_steps[i] = (long)(m_hysteresis_mm[i] * axis_steps_per_unit[i]);
+  }
+
+  //===========================================================================
+  void Hysteresis::ReportToSerial() {
+    ECHO_SMV(DB, "Hysteresis X", m_hysteresis_mm[X_AXIS]);
+    ECHO_MV(" Y", m_hysteresis_mm[Y_AXIS]);
+    ECHO_MV(" Z", m_hysteresis_mm[Z_AXIS]);
+    ECHO_MV(" E", m_hysteresis_mm[E_AXIS]);
+    ECHO_MV(" SHIFTS: x=", axis_shift[X_AXIS]);
+    ECHO_MV(" y=", axis_shift[Y_AXIS]);
+    ECHO_MV(" z=", axis_shift[Z_AXIS]);
+    ECHO_EMV(" e=", axis_shift[E_AXIS]);
+  }
+
+  //===========================================================================
+  // direction 0: positive, 1: negative
+  uint8_t calc_direction_bits(const long* current_position, const long* destination) {
+    unsigned char direction_bits = 0;
+
+    if (destination[X_AXIS] < current_position[X_AXIS])
+      direction_bits |= (1 << X_AXIS);
+    if (destination[Y_AXIS] < current_position[Y_AXIS])
+      direction_bits |= (1 << Y_AXIS);
+    if (destination[Z_AXIS] < current_position[Z_AXIS])
+      direction_bits |= (1 << Z_AXIS);
+    if (destination[E_AXIS] < current_position[E_AXIS])
+      direction_bits |= (1 << E_AXIS);
+
+    return direction_bits;
+  }
+
+  //===========================================================================
+  uint8_t calc_move_bits(const long* current_position, const long* destination) {
+    uint8_t move_bits = 0;
+
+    if (destination[X_AXIS] != current_position[X_AXIS])
+      move_bits |= (1 << X_AXIS);
+    if (destination[Y_AXIS] != current_position[Y_AXIS])
+      move_bits |= (1 << Y_AXIS);
+    if (destination[Z_AXIS] != current_position[Z_AXIS])
+      move_bits |= (1 << Z_AXIS);
+    if (destination[E_AXIS] != current_position[E_AXIS])
+      move_bits |= (1 << E_AXIS);
+
+    return move_bits;
+  }
+
+  //===========================================================================
+  // insert a plan_buffer_line if required to handle any hysteresis
+  void Hysteresis::InsertCorrection(const float x, const float y, const float z, const float e) {
+    long destination[NUM_AXIS] = {x * axis_steps_per_unit[X_AXIS], y * axis_steps_per_unit[Y_AXIS], z * axis_steps_per_unit[Z_AXIS], e * axis_steps_per_unit[E_AXIS + active_extruder]};
+    uint8_t direction_bits = calc_direction_bits(position, destination);
+    uint8_t move_bits = calc_move_bits(position, destination);
+
+    // if the direction has changed in any of the axis that need hysteresis corrections...
+    uint8_t direction_change_bits = (direction_bits ^ m_prev_direction_bits) & move_bits;
+
+    if( (direction_change_bits & m_hysteresis_bits) != 0 ) {
+      // calculate the position to move to that will fix the hysteresis
+      for(uint8_t axis = 0; axis < NUM_AXIS; axis++) {
+        // if this axis changed direction...
+        if(direction_change_bits & (1 << axis)) {
+          long fix = (((direction_bits & (1 << axis)) != 0) ? -m_hysteresis_steps[axis] : m_hysteresis_steps[axis]);
+          //... add the hysteresis: move the current position in the opposite direction so that the next travel move is longer
+          position[axis] -= fix;
+          axis_shift[axis] += fix;
+        }
+      }
+    }
+    m_prev_direction_bits = (direction_bits & move_bits) | (m_prev_direction_bits & ~move_bits);
+  }
+
+#endif // HYSTERESIS
+
+#ifdef ZWOBBLE
+  // minimum distance within which two distances in mm are considered equal
+  #define TOLERANCE_MM 0.01
+  #define DISTANCE(_A,_B) abs((_A) - (_B))
+  #define EQUAL_WITHIN_TOLERANCE(_A, _B) (DISTANCE(_A, _B) < TOLERANCE_MM)
+  #define TWOPI 6.28318530718
+  #define ZACTUAL_IS_SCALED(_I) (zLut[_I][1] < 0)
+  #define ZACTUAL(_I) (zLut[_I][1] < 0 ? -zLut[_I][1] * m_scalingFactor : zLut[_I][1])
+  #define ZROD(_I) zLut[_I][0]
+  #define SET_ZACTUAL(_I,_V) zLut[_I][1] = _V
+  #define SET_ZROD(_I,_V) zLut[_I][0] = _V
+
+  //===========================================================================
+  ZWobble zwobble(DEFAULT_ZWOBBLE);
+
+  //===========================================================================
+  ZWobble::ZWobble(float _amplitude, float _period, float _phase) :
+    m_consistent(false),
+    lastZ(-1.0),
+    lastZRod(-1.0),
+    m_scalingFactor(1.0),
+    m_sinusoidal(true) { Set(_amplitude, _period, _phase); }
+
+  //===========================================================================
+  void ZWobble::Set(float _amplitude, float _period, float _phase) {
+    setAmplitude(_amplitude);
+    setPeriod(_period);
+    setPhase(_phase);
+  }
+
+  //===========================================================================
+  bool ZWobble::areParametersConsistent() {
+    if (!m_sinusoidal) {
+      m_consistent = true;  // parameters are always consistent if lut is not sinusoidal
+      return true;          // if the model is not sinusoidal, then don't check for consistency
+    }
+
+     // m_amplitude*m_puls must be less than 1 in order for the function to be invertible (otherwise it would mean that the wobble is so much that the axis goes back)
+    if (m_puls <= 0 || m_amplitude <= 0 || (m_amplitude * m_puls) >= 1) {
+      m_consistent = false;
+      return false;
+    }
+
+    m_consistent = true;
+    return true;
+  }
+
+  //===========================================================================
+  void ZWobble::setScaledSample(float zRod, float zScaledLength) {
+    // We want to be able to correct scaling factor or set it before/after samples, so (ICK) store scaled samples as negative numbers  
+    setSample(zRod, -zScaledLength);
+
+    // Make sure we have a non-zero scaling factor
+    if (!m_scalingFactor) {
+      m_scalingFactor = 1.0;
+    }
+
+    // Find two scaled samples close to period
+    float period = TWOPI / m_puls;
+    int s1 = -1, s2 = -1;
+
+    for (int i = 0; i < lutSize; i++) {
+      if (ZACTUAL_IS_SCALED(i)) {
+        s1 = s2;
+        s2 = i;
+        if (ZROD(s2) >= period) break;
+      }
+    }
+
+    // Calculate scaling factor so zact[period] = zrod[period]
+    if (s2 >= 0 && ZACTUAL(s2)) {
+      // Case 1 - Only one sample
+      if (s1 < 0) {
+        m_scalingFactor *= ZROD(s2) / ZACTUAL(s2);
+      }
+
+      // Case 2 - Samples bracketing period (s1 - p - s2): average zact
+      else if (ZROD(s2) > period) {
+        float gap1 = period - ZROD(s1);
+        float gap2 = ZROD(s2) - period;
+        float zActPeriod = ZACTUAL(s1) + (ZACTUAL(s2) - ZACTUAL(s1)) * gap1 / (gap1 + gap2);
+
+        m_scalingFactor *= period / zActPeriod;
+      }
+
+      // Case 3 - Both samples before period (s1 - s2 - p): extrapolate zact
+      else {
+        float gap1 = ZROD(s2) - ZROD(s1);
+        float gap2 = period - ZROD(s2);
+        float zActPeriod = ZACTUAL(s2) + (ZACTUAL(s2) - ZACTUAL(s1)) * gap2 / gap1;
+
+        m_scalingFactor *= period / zActPeriod;
+      }
+    }
+  }
+
+  //===========================================================================
+  void ZWobble::setScalingFactor(float zActualPerScaledLength) {
+    m_scalingFactor = zActualPerScaledLength;
+  }
+
+  //===========================================================================
+  void ZWobble::setSample(float zRod, float zActual) {
+    if (debugLevel & DEBUG_DEBUG) {
+      ECHO_SMV(DB, "New sample Rod: ", zRod);
+      ECHO_EMV(" Act: ", zActual);
+    }
+
+    if (m_puls <= 0) {
+      ECHO_LM(DB, "You must define a period first (M97 W...)");
+      return;
+    }
+
+    if (m_sinusoidal) {
+      m_sinusoidal = false;
+      calculateLut(); // initializes the LUT to linear
+    }
+    insertInLut(zRod, zActual);
+  }
+
+  //===========================================================================
+  void ZWobble::insertInLut(float zRod, float zActual) {
+    // check if the given zRod alread exists in LUT
+    for (int i = 0; i < lutSize; i++) {
+      if (EQUAL_WITHIN_TOLERANCE(zRod, ZROD(i))) {
+        // replace value
+        SET_ZROD(i, zRod);
+        SET_ZACTUAL(i, zActual);
+        return;
+      }
+    }
+
+    // ok the value does not exist: is there still space in LUT? Insert it
+    if (lutSize < STEPS_IN_ZLUT) {
+      int zPlace = -1;
+      for (int i = 0; i < lutSize; i++) {
+        if (ZROD(i) > zRod) {
+          zPlace = i;
+          break;
+        }
+      }
+
+      // shift samples after zPlace
+      for (int i = lutSize; i > zPlace; i--) {
+        SET_ZROD(i, ZROD(i - 1));
+        SET_ZACTUAL(i, ZACTUAL(i - 1));
+      }
+      lutSize++; // increase lutSize
+
+      // insert sample
+      SET_ZROD(zPlace, zRod);
+      SET_ZACTUAL(zPlace, zActual);
+      return;
+    }
+    else {
+      // lutSize == STEPS_IN_ZLUT: replace the closest point with the new sample
+      int zPlace = 0;
+      float dist = DISTANCE(zRod, ZROD(zPlace));
+      for (int i = 1; i < lutSize; i++) {
+        if (DISTANCE(zRod, ZROD(i)) < dist) {
+          zPlace = i;
+          dist = DISTANCE(zRod, ZROD(i));
+        }
+      }
+
+      SET_ZROD(zPlace, zRod);
+      SET_ZACTUAL(zPlace, zActual);
+    }
+  }
+
+  //===========================================================================
+  void ZWobble::initLinearLut() {
+    float period = TWOPI / m_puls;
+    lutSize = 2; // only 2 samples originally
+    SET_ZROD(0, 0);
+    SET_ZACTUAL(0, 0);
+    SET_ZROD(1, period);
+    SET_ZACTUAL(1, period);
+  }
+
+  //===========================================================================
+  // calculate the ZRod -> Zactual LUT using the model Zactual = Zrod + sin(w*Zrod) - this will actually only be used for one period
+  void ZWobble::calculateLut() {
+    lastZ = -1.0;
+    lastZRod = -1.0; // reinitialize memorized Z values since we are changing the model
+    if (!areParametersConsistent()) return;
+    if (!m_sinusoidal) {
+      initLinearLut();
+      return; // if the model is not sinusoidal, initializes LUT to linear
+    }
+    lutSize = STEPS_IN_ZLUT;
+    float period = TWOPI / m_puls;
+    // divide the period in STEPS_IN_ZLUT steps
+    float lutStep = period / STEPS_IN_ZLUT;
+    for (int i = 0; i < STEPS_IN_ZLUT; i++) {
+      float zRod = lutStep * i;
+      SET_ZROD(i, zRod);
+      SET_ZACTUAL(i, zRod + m_amplitude * sin(m_puls * zRod));
+    }
+  }
+
+  //===========================================================================
+  void ZWobble::setAmplitude(float _amplitude) {
+    m_amplitude = _amplitude;
+    m_sinusoidal = true; // setAmplitude sets to sinusoidal by default
+    calculateLut();
+  }
+
+  //===========================================================================
+  void ZWobble::setPeriod(float _period) {
+    if (_period <= 0) return;
+    m_puls = TWOPI/_period;
+    calculateLut();
+  }
+
+  //===========================================================================
+  void ZWobble::setPhase(float _phase ) {
+    // poor man's modulo operation
+    while (_phase > 0) _phase -= 360;
+    while (_phase < 0) _phase += 360;
+
+    // phase now will be between 0 and 360
+    m_phase = (_phase * M_PI / 180); // convert phase to radians
+  }
+
+  //===========================================================================
+  void ZWobble::ReportToSerial() {
+    if (!m_sinusoidal)
+      ECHO_SM(DB, "Custom wobble function");
+    else {
+      ECHO_SMV(DB, "ZWobble Amp(A): ", m_amplitude);
+    }
+
+    ECHO_MV(" phase(P): ", m_phase); 
+    ECHO_MV(" period(W): ", TWOPI / m_puls);
+    ECHO_MV(" puls: ", m_puls);
+
+    if (!areParametersConsistent())
+      ECHO_M(" Warning! Inconsistent parameters!");
+
+    ECHO_E;
+
+    if (!m_sinusoidal) {
+      // print out the LUT
+      for (int i = 0; i < lutSize; i++) {
+        ECHO_SMV(DB, "Rod: ", ZROD(i));
+        ECHO_MV(" Act: ", ZACTUAL(i));
+
+        int delta = (ZACTUAL(i) - ZROD(i)) * 200 + 20;
+        for (int j = 0; j < delta; j++) {
+          ECHO_M(" ");
+        }
+        ECHO_EM("  +");
+      }
+    }
+  }
+
+  //===========================================================================
+  float ZWobble::findInLut(float z) {
+    int i = 0;
+    if (z >= ZACTUAL(lutSize - 1))
+      return ZROD(lutSize - 1);
+
+    if (z <= ZACTUAL(0))
+      return ZROD(0);
+
+    for (i = 0; i < lutSize; i++) {
+      if (ZACTUAL(i) > z)
+        break;
+    }
+
+    float invZDist = 1 / (ZACTUAL(i) - ZACTUAL(i-1)); // distance between Z steps
+
+    float interpZ = (ZROD(i - 1) * (ZACTUAL(i) - z) + ZROD(i) * (z - ZACTUAL(i - 1))) * invZDist; // linear interpolation between neighboring Z values
+
+    return interpZ;
+  }
+
+  //===========================================================================
+  // Find the Z value to be given to the "rod" in order to obtain the desired Z
+  float ZWobble::findZRod(float z) {
+    int nCycle = 0;
+    float identicalZ = -m_phase / m_puls;
+
+    // find the last point in which the two Z are identical: this happens every (2kPI-phase)/w
+    while (identicalZ <= z) identicalZ = (TWOPI * (++nCycle) - m_phase) / m_puls; 
+
+    // find Z again using the previous cycle
+    identicalZ = (TWOPI *(nCycle - 1) - m_phase) / m_puls;
+
+    float deltaZa = z - identicalZ;
+
+    // find deltaZRod by linear interpolation of the lookup table
+    float deltaZrod = findInLut(deltaZa);
+    return identicalZ + deltaZrod;
+  }
+
+  //===========================================================================
+  // insert a plan_buffer_line if required to handle any hysteresis
+  void ZWobble::InsertCorrection(const float targetZ) {
+
+    if (!m_consistent) return; // don't go through consistency checks all the time; just check one bool
+
+    float originZ = (float)position[Z_AXIS] / axis_steps_per_unit[Z_AXIS];
+
+    if (originZ < ZWOBBLE_MIN_Z || targetZ < ZWOBBLE_MIN_Z) return;
+
+    if (debugLevel & DEBUG_DEBUG) {
+      ECHO_SMV(DB, "Origin: ", originZ);
+      ECHO_MV(" Target: ", targetZ);
+    }
+
+    if (EQUAL_WITHIN_TOLERANCE(originZ, targetZ)) return; // if there is no Z move, do nothing
+
+    float originZRod;
+
+    // there is a high chance that the origin Z is the same as the last target Z: skip one iteration of the algorithm if possible
+    if (originZ == lastZ)
+      originZRod = lastZRod;
+    else
+      originZRod = findZRod(originZ);
+    
+    if (debugLevel & DEBUG_DEBUG)
+      ECHO_MV(" Origin rod: ", originZRod);
+
+    float targetZRod = findZRod(targetZ);
+
+    if (debugLevel & DEBUG_DEBUG)
+      ECHO_MV(" Target Rod: ", targetZRod);
+
+    // difference in steps between the correct movement (originZRod->targetZRod) and the planned movement
+    long stepDiff = lround((targetZRod - originZRod) * axis_steps_per_unit[Z_AXIS]) - (lround(targetZ * axis_steps_per_unit[Z_AXIS]) - position[Z_AXIS]);
+
+    if (debugLevel & DEBUG_DEBUG)
+      ECHO_EMV(" stepDiff: ", stepDiff);
+
+    lastZ = targetZ;
+    lastZRod = targetZRod;
+   
+    // don't adjust if target posizion is less than 0
+    if (position[Z_AXIS] - stepDiff > 0)
+      position[Z_AXIS] -= stepDiff;
+  }
+#endif

MK/module/motion/cartesian_correction.h

+/**
+ * cartesian_correction.h
+ * A class that manages hysteresis by inserting extra plan_buffer_line when necessary
+ * A class that manages ZWobble
+ *
+ * Copyright (c) 2016 MagoKimbra
+ * Copyright (c) 2013 Francesco Santini
+ * Copyright (c) 2012 Neil James Martin
+ *
+ * Grbl is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Grbl is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _CARTESIAN_CORRECTION_H
+  #define _CARTESIAN_CORRECTION_H
+
+  #ifdef HYSTERESIS
+    //===========================================================================
+    class Hysteresis {
+    public:
+      Hysteresis(float x_mm, float y_mm, float z_mm, float e_mm);
+
+      void Set(float x_mm, float y_mm, float z_mm, float e_mm);
+      void SetAxis(uint8_t axis, float mm);
+      void ReportToSerial();
+      void InsertCorrection(const float x, const float y, const float z, const float e);
+
+    private:
+      void      calcSteps();
+      float     m_hysteresis_mm[NUM_AXIS];
+      long      m_hysteresis_steps[NUM_AXIS];
+      uint8_t   m_prev_direction_bits;
+      uint8_t   m_hysteresis_bits;
+    };
+
+    //===========================================================================
+    extern Hysteresis hysteresis;
+  #endif // HYSTERESIS
+
+  #ifdef ZWOBBLE
+    #define STEPS_IN_ZLUT 50
+    #define ZWOBBLE_MIN_Z 0.1
+    //===========================================================================
+    class ZWobble {
+    public:
+      ZWobble(float _amplitude, float _period, float _phase);
+
+      void Set(float _amplitude, float _period, float _phase);
+      void ReportToSerial();
+      void InsertCorrection(const float targetZ);
+
+      void setAmplitude(float _amplitude);
+      void setPeriod(float _period);
+      void setPhase(float _phase);
+      void setSample(float zRod, float zActual);
+      void setScaledSample(float zRod, float zScaledLength);
+      void setScalingFactor(float zActualPerScaledLength);
+
+    private:
+      float     m_amplitude, m_puls, m_phase;
+      bool      m_consistent;
+
+      int       lutSize;
+      float     zLut[STEPS_IN_ZLUT][2];
+      void      calculateLut();
+      void      initLinearLut();
+      void      insertInLut(float, float);
+      float     findInLut(float);
+      float     findZRod(float);
+      bool      areParametersConsistent();
+
+      float     lastZ, lastZRod;
+      float     m_scalingFactor;
+      bool      m_sinusoidal;
+
+    };
+
+    //===========================================================================
+    extern ZWobble zwobble;
+  #endif // ZWOBBLE
+
+#endif // _CARTESIAN_CORRECTION_H

MK/module/motion/planner.cpp

@@ -487,6 +487,16 @@ float junction_deviation = 0.1;
   void plan_buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder, const uint8_t driver)
 #endif  // AUTO_BED_LEVELING_FEATURE
 {
+
+  #if ENABLED(ZWOBBLE)
+    // Calculate ZWobble
+    zwobble.InsertCorrection(z);
+  #endif
+  #if ENABLED(HYSTERESIS)
+    // Calculate Hysteresis
+    hysteresis.InsertCorrection(x, y, z, e);
+  #endif
+
   // Calculate the buffer head after we push this byte
   int next_buffer_head = next_block_index(block_buffer_head);
 

MK/module/motion/planner.h

@@ -140,6 +140,7 @@ extern float max_z_jerk;
 extern float max_e_jerk[EXTRUDERS];
 extern float mintravelfeedrate;
 extern unsigned long axis_steps_per_sqr_second[3 + EXTRUDERS];
+extern long position[NUM_AXIS];
 
 #if ENABLED(AUTOTEMP)
   extern bool autotemp_enabled;

MK/module/motion/qr_solve.cpp

 #include "../../base.h"
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE) && ENABLED(AUTO_BED_LEVELING_GRID)
+  #include "qr_solve.h"
+  #include <stdlib.h>
+  #include <math.h>
+  //# include "r8lib.h"
 
-#include "qr_solve.h"
-#include <stdlib.h>
-#include <math.h>
+  int i4_min(int i1, int i2)
 
-//# include "r8lib.h"
-
-int i4_min(int i1, int i2)
-
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       I4_MIN returns the smaller of two I4's.
@@ -33,15 +31,15 @@ int i4_min(int i1, int i2)
       Input, int I1, I2, two integers to be compared.
 
       Output, int I4_MIN, the smaller of I1 and I2.
-*/
-{
+  */
+  {
     return (i1 < i2) ? i1 : i2;
-}
+  }
 
-double r8_epsilon(void)
+  double r8_epsilon(void)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8_EPSILON returns the R8 round off unit.
@@ -69,16 +67,16 @@ double r8_epsilon(void)
     Parameters:
 
       Output, double R8_EPSILON, the R8 round-off unit.
-*/
-{
+  */
+  {
     const double value = 2.220446049250313E-016;
     return value;
-}
+  }
 
-double r8_max(double x, double y)
+  double r8_max(double x, double y)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8_MAX returns the maximum of two R8's.
@@ -100,15 +98,15 @@ double r8_max(double x, double y)
       Input, double X, Y, the quantities to compare.
 
       Output, double R8_MAX, the maximum of X and Y.
-*/
-{
+  */
+  {
     return (y < x) ? x : y;
-}
+  }
 
-double r8_abs(double x)
+  double r8_abs(double x)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8_ABS returns the absolute value of an R8.
@@ -130,15 +128,15 @@ double r8_abs(double x)
       Input, double X, the quantity whose absolute value is desired.
 
       Output, double R8_ABS, the absolute value of X.
-*/
-{
+  */
+  {
     return (x < 0.0) ? -x : x;
-}
+  }
 
-double r8_sign(double x)
+  double r8_sign(double x)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8_SIGN returns the sign of an R8.
@@ -160,15 +158,15 @@ double r8_sign(double x)
       Input, double X, the number whose sign is desired.
 
       Output, double R8_SIGN, the sign of X.
-*/
-{
+  */
+  {
     return (x < 0.0) ? -1.0 : 1.0;
-}
+  }
 
-double r8mat_amax(int m, int n, double a[])
+  double r8mat_amax(int m, int n, double a[])
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8MAT_AMAX returns the maximum absolute value entry of an R8MAT.
@@ -199,8 +197,8 @@ double r8mat_amax(int m, int n, double a[])
       Input, double A[M*N], the M by N matrix.
 
       Output, double R8MAT_AMAX, the maximum absolute value entry of A.
-*/
-{
+  */
+  {
     double value = r8_abs(a[0 + 0 * m]);
     for (int j = 0; j < n; j++) {
       for (int i = 0; i < m; i++) {
@@ -208,12 +206,12 @@ double r8mat_amax(int m, int n, double a[])
       }
     }
     return value;
-}
+  }
 
-void r8mat_copy(double a2[], int m, int n, double a1[])
+  void r8mat_copy(double a2[], int m, int n, double a1[])
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       R8MAT_COPY_NEW copies one R8MAT to a "new" R8MAT.
@@ -242,20 +240,20 @@ void r8mat_copy(double a2[], int m, int n, double a1[])
       Input, double A1[M*N], the matrix to be copied.
 
       Output, double R8MAT_COPY_NEW[M*N], the copy of A1.
-*/
-{
+  */
+  {
     for (int j = 0; j < n; j++) {
       for (int i = 0; i < m; i++)
         a2[i + j * m] = a1[i + j * m];
     }
-}
+  }
 
-/******************************************************************************/
+  /******************************************************************************/
 
-void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
+  void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DAXPY computes constant times a vector plus a vector.
@@ -302,8 +300,8 @@ void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
       On output, DY[*] has been replaced by DY[*] + DA * DX[*].
 
       Input, int INCY, the increment between successive entries of DY.
-*/
-{
+  */
+  {
     if (n <= 0 || da == 0.0) return;
 
     int i, ix, iy, m;
@@ -340,13 +338,13 @@ void daxpy(int n, double da, double dx[], int incx, double dy[], int incy)
         dy[i + 3] = dy[i + 3] + da * dx[i + 3];
       }
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-double ddot(int n, double dx[], int incx, double dy[], int incy)
+  double ddot(int n, double dx[], int incx, double dy[], int incy)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DDOT forms the dot product of two vectors.
@@ -393,8 +391,8 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
 
       Output, double DDOT, the sum of the product of the corresponding
       entries of DX and DY.
-*/
-{
+  */
+  {
 
     if (n <= 0) return 0.0;
 
@@ -430,13 +428,13 @@ double ddot(int n, double dx[], int incx, double dy[], int incy)
       }
     }
     return dtemp;
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-double dnrm2(int n, double x[], int incx)
+  double dnrm2(int n, double x[], int incx)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DNRM2 returns the euclidean norm of a vector.
@@ -478,8 +476,8 @@ double dnrm2(int n, double x[], int incx)
       Input, int INCX, the increment between successive entries of X.
 
       Output, double DNRM2, the Euclidean norm of X.
-*/
-{
+  */
+  {
     double norm;
     if (n < 1 || incx < 1)
       norm = 0.0;
@@ -503,14 +501,14 @@ double dnrm2(int n, double x[], int incx)
       norm = scale * sqrt(ssq);
     }
     return norm;
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
+  void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
               int jpvt[], double qraux[])
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DQRANK computes the QR factorization of a rectangular matrix.
@@ -579,8 +577,8 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
 
       Output, double QRAUX[N], will contain extra information defining
       the QR factorization.
-*/
-{
+  */
+  {
     double work[n];
 
     for (int i = 0; i < n; i++)
@@ -597,14 +595,14 @@ void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
         return;
       *kr = j + 1;
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
+  void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
              double work[], int job)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DQRDC computes the QR factorization of a real rectangular matrix.
@@ -679,8 +677,8 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
       Input, int JOB, initiates column pivoting.
       0, no pivoting is done.
       nonzero, pivoting is done.
-*/
-{
+  */
+  {
     int jp;
     int j;
     int lup;
@@ -791,14 +789,14 @@ void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
         }
       }
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
+  int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
             double x[], double rsd[], int jpvt[], double qraux[], int itask)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DQRLS factors and solves a linear system in the least squares sense.
@@ -900,8 +898,8 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
       -1: LDA < M   (fatal error)
       -2: N < 1     (fatal error)
       -3: ITASK < 1 (fatal error)
-*/
-{
+  */
+  {
     int ind;
     if (lda < m) {
       /*fprintf ( stderr, "\n" );
@@ -938,14 +936,14 @@ int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
     */
     dqrlss(a, lda, m, n, *kr, b, x, rsd, jpvt, qraux);
     return ind;
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
+  void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
               double rsd[], int jpvt[], double qraux[])
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DQRLSS solves a linear system in a least squares sense.
@@ -1013,8 +1011,8 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
 
       Input, double QRAUX[N], auxiliary information from DQRANK
       defining the QR factorization.
-*/
-{
+  */
+  {
     int i;
     int info;
     int j;
@@ -1048,14 +1046,14 @@ void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
         }
       }
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
+  int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
             double qy[], double qty[], double b[], double rsd[], double ab[], int job)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DQRSL computes transformations, projections, and least squares solutions.
@@ -1188,8 +1186,8 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
       Output, int DQRSL, is zero unless the computation of B has
       been requested and R is exactly singular.  In this case, INFO is the
       index of the first zero diagonal element of R, and B is left unaltered.
-*/
-{
+  */
+  {
     int cab;
     int cb;
     int cqty;
@@ -1341,15 +1339,15 @@ int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
       }
     }
     return info;
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-/******************************************************************************/
+  /******************************************************************************/
 
-void dscal(int n, double sa, double x[], int incx)
+  void dscal(int n, double sa, double x[], int incx)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DSCAL scales a vector by a constant.
@@ -1387,8 +1385,8 @@ void dscal(int n, double sa, double x[], int incx)
       Input/output, double X[*], the vector to be scaled.
 
       Input, int INCX, the increment between successive entries of X.
-*/
-{
+  */
+  {
     int i;
     int ix;
     int m;
@@ -1417,14 +1415,14 @@ void dscal(int n, double sa, double x[], int incx)
         ix = ix + incx;
       }
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
 
-void dswap(int n, double x[], int incx, double y[], int incy)
+  void dswap(int n, double x[], int incx, double y[], int incy)
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       DSWAP interchanges two vectors.
@@ -1464,8 +1462,8 @@ void dswap(int n, double x[], int incx, double y[], int incy)
       Input/output, double Y[*], one of the vectors to swap.
 
       Input, int INCY, the increment between successive elements of Y.
-*/
-{
+  */
+  {
     if (n <= 0) return;
 
     int i, ix, iy, m;
@@ -1501,15 +1499,15 @@ void dswap(int n, double x[], int incx, double y[], int incy)
         iy = iy + incy;
       }
     }
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
-/******************************************************************************/
+  /******************************************************************************/
 
-void qr_solve(double x[], int m, int n, double a[], double b[])
+  void qr_solve(double x[], int m, int n, double a[], double b[])
 
-/******************************************************************************/
-/*
+  /******************************************************************************/
+  /*
     Purpose:
 
       QR_SOLVE solves a linear system in the least squares sense.
@@ -1554,8 +1552,8 @@ void qr_solve(double x[], int m, int n, double a[], double b[])
       Input, double B[M], the right hand side.
 
       Output, double QR_SOLVE[N], the least squares solution.
-*/
-{
+  */
+  {
     double a_qr[n * m], qraux[n], r[m], tol;
     int ind, itask, jpvt[n], kr, lda;
 
@@ -1565,7 +1563,7 @@ void qr_solve(double x[], int m, int n, double a[], double b[])
     itask = 1;
 
     ind = dqrls ( a_qr, lda, m, n, tol, &kr, b, x, r, jpvt, qraux, itask ); UNUSED(ind);
-}
-/******************************************************************************/
+  }
+  /******************************************************************************/
 
 #endif

MK/module/motion/qr_solve.h

 #if ENABLED(AUTO_BED_LEVELING_GRID)
 
-void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
-double ddot(int n, double dx[], int incx, double dy[], int incy);
-double dnrm2(int n, double x[], int incx);
-void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
+  void daxpy(int n, double da, double dx[], int incx, double dy[], int incy);
+  double ddot(int n, double dx[], int incx, double dy[], int incy);
+  double dnrm2(int n, double x[], int incx);
+  void dqrank(double a[], int lda, int m, int n, double tol, int* kr,
               int jpvt[], double qraux[]);
-void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
+  void dqrdc(double a[], int lda, int n, int p, double qraux[], int jpvt[],
              double work[], int job);
-int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
+  int dqrls(double a[], int lda, int m, int n, double tol, int* kr, double b[],
             double x[], double rsd[], int jpvt[], double qraux[], int itask);
-void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
+  void dqrlss(double a[], int lda, int m, int n, int kr, double b[], double x[],
               double rsd[], int jpvt[], double qraux[]);
-int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
+  int dqrsl(double a[], int lda, int n, int k, double qraux[], double y[],
             double qy[], double qty[], double b[], double rsd[], double ab[], int job);
-void dscal(int n, double sa, double x[], int incx);
-void dswap(int n, double x[], int incx, double y[], int incy);
-void qr_solve(double x[], int m, int n, double a[], double b[]);
+  void dscal(int n, double sa, double x[], int incx);
+  void dswap(int n, double x[], int incx, double y[], int incy);
+  void qr_solve(double x[], int m, int n, double a[], double b[]);
 
 #endif

MK/module/motion/vector_3.cpp

-/*
-  vector_3.cpp - Vector library for bed leveling
-  Copyright (c) 2012 Lars Brubaker.  All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-*/
+/**
+ * vector_3.cpp - Vector library for bed leveling
+ * Copyright (c) 2012 Lars Brubaker.  All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
 
 #include "../../base.h"
-#include <math.h>
 
 #if ENABLED(AUTO_BED_LEVELING_FEATURE)
-#include "vector_3.h"
+  #include <math.h>
+  #include "vector_3.h"
 
-vector_3::vector_3() : x(0), y(0), z(0) { }
+  vector_3::vector_3() : x(0), y(0), z(0) { }
 
-vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
+  vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
 
-vector_3 vector_3::cross(vector_3 left, vector_3 right) {
+  vector_3 vector_3::cross(vector_3 left, vector_3 right) {
     return vector_3(left.y * right.z - left.z * right.y,
                     left.z * right.x - left.x * right.z,
                     left.x * right.y - left.y * right.x);
-}
+  }
 
-vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
-vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
+  vector_3 vector_3::operator+(vector_3 v) { return vector_3((x + v.x), (y + v.y), (z + v.z)); }
+  vector_3 vector_3::operator-(vector_3 v) { return vector_3((x - v.x), (y - v.y), (z - v.z)); }
 
-vector_3 vector_3::get_normal() {
+  vector_3 vector_3::get_normal() {
     vector_3 normalized = vector_3(x, y, z);
     normalized.normalize();
     return normalized;
-}
+  }
 
-float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
+  float vector_3::get_length() { return sqrt((x * x) + (y * y) + (z * z)); }
 
-void vector_3::normalize() {
+  void vector_3::normalize() {
     float length = get_length();
     x /= length;
     y /= length;
     z /= length;
-}
+  }
 
-void vector_3::apply_rotation(matrix_3x3 matrix) {
+  void vector_3::apply_rotation(matrix_3x3 matrix) {
     float resultX = x * matrix.matrix[0][0] + y * matrix.matrix[1][0] + z * matrix.matrix[2][0];
     float resultY = x * matrix.matrix[0][1] + y * matrix.matrix[1][1] + z * matrix.matrix[2][1];
     float resultZ = x * matrix.matrix[0][2] + y * matrix.matrix[1][2] + z * matrix.matrix[2][2];
     x = resultX;
     y = resultY;
     z = resultZ;
-}
+  }
 
-void vector_3::debug(const char title[]) {
+  void vector_3::debug(const char title[]) {
     ECHO_ST(DB, title);
     ECHO_MV(" x: ", x, 6);
     ECHO_MV(" y: ", y, 6);
     ECHO_EMV(" z: ", z, 6);
-}
+  }
 
-void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
+  void apply_rotation_xyz(matrix_3x3 matrix, float& x, float& y, float& z) {
     vector_3 vector = vector_3(x, y, z);
     vector.apply_rotation(matrix);
     x = vector.x;
     y = vector.y;
     z = vector.z;
-}
+  }
 
-matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
+  matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2) {
     //row_0.debug("row_0");
     //row_1.debug("row_1");
     //row_2.debug("row_2");
@@ -85,15 +85,15 @@ matrix_3x3 matrix_3x3::create_from_rows(vector_3 row_0, vector_3 row_1, vector_3
     new_matrix.matrix[2][0] = row_2.x; new_matrix.matrix[2][1] = row_2.y; new_matrix.matrix[2][2] = row_2.z;
     //new_matrix.debug("new_matrix");
     return new_matrix;
-}
+  }
 
-void matrix_3x3::set_to_identity() {
+  void matrix_3x3::set_to_identity() {
     matrix[0][0] = 1; matrix[0][1] = 0; matrix[0][2] = 0;
     matrix[1][0] = 0; matrix[1][1] = 1; matrix[1][2] = 0;
     matrix[2][0] = 0; matrix[2][1] = 0; matrix[2][2] = 1;
-}
+  }
 
-matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
+  matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
     vector_3 z_row = target.get_normal();
     vector_3 x_row = vector_3(1, 0, -target.x / target.z).get_normal();
     vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
@@ -107,17 +107,17 @@ matrix_3x3 matrix_3x3::create_look_at(vector_3 target) {
 
     // rot.debug("rot");
     return rot;
-}
+  }
 
-matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
+  matrix_3x3 matrix_3x3::transpose(matrix_3x3 original) {
     matrix_3x3 new_matrix;
     new_matrix.matrix[0][0] = original.matrix[0][0]; new_matrix.matrix[0][1] = original.matrix[1][0]; new_matrix.matrix[0][2] = original.matrix[2][0];
     new_matrix.matrix[1][0] = original.matrix[0][1]; new_matrix.matrix[1][1] = original.matrix[1][1]; new_matrix.matrix[1][2] = original.matrix[2][1];
     new_matrix.matrix[2][0] = original.matrix[0][2]; new_matrix.matrix[2][1] = original.matrix[1][2]; new_matrix.matrix[2][2] = original.matrix[2][2];
     return new_matrix;
-}
+  }
 
-void matrix_3x3::debug(const char title[]) {
+  void matrix_3x3::debug(const char title[]) {
     ECHO_LT(DB, title);
     for (uint8_t i = 0; i < 3; i++) {
       ECHO_S(DB);
@@ -128,6 +128,6 @@ void matrix_3x3::debug(const char title[]) {
       }
       ECHO_E;
     }
-}
+  }
 
 #endif // AUTO_BED_LEVELING_FEATURE

MK/module/motion/vector_3.h

-/*
-  vector_3.cpp - Vector library for bed leveling
-  Copyright (c) 2012 Lars Brubaker.  All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
-*/
+/**
+ * vector_3.cpp - Vector library for bed leveling
+ * Copyright (c) 2012 Lars Brubaker.  All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+ */
 
 #ifndef VECTOR_3_H
-#define VECTOR_3_H
+  #define VECTOR_3_H
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-class matrix_3x3;
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+    class matrix_3x3;
 
-struct vector_3 {
+    struct vector_3 {
       float x, y, z;
 
       vector_3();
@@ -40,9 +40,9 @@ struct vector_3 {
       void debug(const char title[]);
 
       void apply_rotation(matrix_3x3 matrix);
-};
+    };
 
-struct matrix_3x3 {
+    struct matrix_3x3 {
       float matrix[3][3];
 
       static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
@@ -50,12 +50,10 @@ struct matrix_3x3 {
       static matrix_3x3 transpose(matrix_3x3 original);
 
       void set_to_identity();
-
       void debug(const char title[]);
-};
-
+    };
 
-void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
-#endif // AUTO_BED_LEVELING_FEATURE
+    void apply_rotation_xyz(matrix_3x3 rotationMatrix, float& x, float& y, float& z);
 
+  #endif // AUTO_BED_LEVELING_FEATURE
 #endif // VECTOR_3_H

MK/module/servo/ServoTimers.h

-/*
-  Copyright (c) 2013 Arduino LLC. All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-*/
+/**
+ * Copyright (c) 2013 Arduino LLC. All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
 
-/*
+/**
  * Defines for 16 bit timers used with  Servo library
  *
  * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
@@ -51,4 +51,3 @@ typedef enum { _timer3, _Nbr_16timers } timer16_Sequence_t;
 #else  // everything else
 typedef enum { _Nbr_16timers } timer16_Sequence_t;
 #endif
-

MK/module/servo/servo.cpp

-/*
-  Copyright (c) 2013 Arduino LLC. All right reserved.
-
-  This library is free software; you can redistribute it and/or
-  modify it under the terms of the GNU Lesser General Public
-  License as published by the Free Software Foundation; either
-  version 2.1 of the License, or (at your option) any later version.
-
-  This library is distributed in the hope that it will be useful,
-  but WITHOUT ANY WARRANTY; without even the implied warranty of
-  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-  Lesser General Public License for more details.
-
-  You should have received a copy of the GNU Lesser General Public
-  License along with this library; if not, write to the Free Software
-  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-*/
+/**
+ * Copyright (c) 2013 Arduino LLC. All right reserved.
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
+ */
 
 #include "../../base.h"
 
 #if HAS(SERVOS)
-#include "servo.h"
+  #include "servo.h"
 
-#define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
-#define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
+  #define usToTicks(_us)    (( clockCyclesPerMicrosecond()* _us) / 8)     // converts microseconds to tick (assumes prescale of 8)  // 12 Aug 2009
+  #define ticksToUs(_ticks) (( (unsigned)_ticks * 8)/ clockCyclesPerMicrosecond() ) // converts from ticks back to microseconds
 
-#define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays
+  #define TRIM_DURATION       2                               // compensation ticks to trim adjust for digitalWrite delays
 
-static servo_t servos[MAX_SERVOS];                          // static array of servo structures
-static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
+  static servo_t servos[MAX_SERVOS];                          // static array of servo structures
+  static volatile int8_t Channel[_Nbr_16timers ];             // counter for the servo being pulsed for each timer (or -1 if refresh interval)
 
-uint8_t ServoCount = 0;                                     // the total number of attached servos
+  uint8_t ServoCount = 0;                                     // the total number of attached servos
 
-// convenience macros
-#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
-#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
-#define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
-#define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
+  // convenience macros
+  #define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / SERVOS_PER_TIMER)) // returns the timer controlling this servo
+  #define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % SERVOS_PER_TIMER)       // returns the index of the servo on this timer
+  #define SERVO_INDEX(_timer,_channel)  ((_timer*SERVOS_PER_TIMER) + _channel)     // macro to access servo index by timer and channel
+  #define SERVO(_timer,_channel)  (servos[SERVO_INDEX(_timer,_channel)])            // macro to access servo class by timer and channel
 
-#define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
-#define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
+  #define SERVO_MIN() (MIN_PULSE_WIDTH - this->min * 4)  // minimum value in uS for this servo
+  #define SERVO_MAX() (MAX_PULSE_WIDTH - this->max * 4)  // maximum value in uS for this servo
 
-/************ static functions common to all instances ***********************/
+  /************ static functions common to all instances ***********************/
 
-static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
-{
+  static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t *TCNTn, volatile uint16_t* OCRnA)
+  {
     if( Channel[timer] < 0 )
       *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer
     else{
@@ -65,95 +65,95 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t
         *OCRnA = *TCNTn + 4;  // at least REFRESH_INTERVAL has elapsed
       Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel
     }
-}
+  }
 
-#ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
-// Interrupt handlers for Arduino
-#if defined(_useTimer1)
-SIGNAL (TIMER1_COMPA_vect)
-{
+  #ifndef WIRING // Wiring pre-defines signal handlers so don't define any if compiling for the Wiring platform
+  // Interrupt handlers for Arduino
+  #if defined(_useTimer1)
+  SIGNAL (TIMER1_COMPA_vect)
+  {
     handle_interrupts(_timer1, &TCNT1, &OCR1A);
-}
-#endif
+  }
+  #endif
 
-#if defined(_useTimer3)
-SIGNAL (TIMER3_COMPA_vect)
-{
+  #if defined(_useTimer3)
+  SIGNAL (TIMER3_COMPA_vect)
+  {
     handle_interrupts(_timer3, &TCNT3, &OCR3A);
-}
-#endif
+  }
+  #endif
 
-#if defined(_useTimer4)
-SIGNAL (TIMER4_COMPA_vect)
-{
+  #if defined(_useTimer4)
+  SIGNAL (TIMER4_COMPA_vect)
+  {
     handle_interrupts(_timer4, &TCNT4, &OCR4A);
-}
-#endif
+  }
+  #endif
 
-#if defined(_useTimer5)
-SIGNAL (TIMER5_COMPA_vect)
-{
+  #if defined(_useTimer5)
+  SIGNAL (TIMER5_COMPA_vect)
+  {
     handle_interrupts(_timer5, &TCNT5, &OCR5A);
-}
-#endif
+  }
+  #endif
 
-#elif defined WIRING
-// Interrupt handlers for Wiring
-#if defined(_useTimer1)
-void Timer1Service()
-{
+  #elif defined WIRING
+  // Interrupt handlers for Wiring
+  #if defined(_useTimer1)
+  void Timer1Service()
+  {
     handle_interrupts(_timer1, &TCNT1, &OCR1A);
-}
-#endif
-#if defined(_useTimer3)
-void Timer3Service()
-{
+  }
+  #endif
+  #if defined(_useTimer3)
+  void Timer3Service()
+  {
     handle_interrupts(_timer3, &TCNT3, &OCR3A);
-}
-#endif
-#endif
+  }
+  #endif
+  #endif
 
 
-static void initISR(timer16_Sequence_t timer)
-{
-#if defined (_useTimer1)
+  static void initISR(timer16_Sequence_t timer)
+  {
+  #if defined (_useTimer1)
     if(timer == _timer1) {
       TCCR1A = 0;             // normal counting mode
       TCCR1B = _BV(CS11);     // set prescaler of 8
       TCNT1 = 0;              // clear the timer count
-#if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
+  #if defined(__AVR_ATmega8__)|| defined(__AVR_ATmega128__)
       TIFR |= _BV(OCF1A);      // clear any pending interrupts;
       TIMSK |=  _BV(OCIE1A) ;  // enable the output compare interrupt
-#else
+  #else
       // here if not ATmega8 or ATmega128
       TIFR1 |= _BV(OCF1A);     // clear any pending interrupts;
       TIMSK1 |=  _BV(OCIE1A) ; // enable the output compare interrupt
-#endif
-#if defined(WIRING)
+  #endif
+  #if defined(WIRING)
       timerAttach(TIMER1OUTCOMPAREA_INT, Timer1Service);
-#endif
+  #endif
     }
-#endif
+  #endif
 
-#if defined (_useTimer3)
+  #if defined (_useTimer3)
     if(timer == _timer3) {
       TCCR3A = 0;             // normal counting mode
       TCCR3B = _BV(CS31);     // set prescaler of 8
       TCNT3 = 0;              // clear the timer count
-#if defined(__AVR_ATmega128__)
+  #if defined(__AVR_ATmega128__)
       TIFR |= _BV(OCF3A);     // clear any pending interrupts;
     ETIMSK |= _BV(OCIE3A);  // enable the output compare interrupt
-#else
+  #else
       TIFR3 = _BV(OCF3A);     // clear any pending interrupts;
       TIMSK3 =  _BV(OCIE3A) ; // enable the output compare interrupt
-#endif
-#if defined(WIRING)
+  #endif
+  #if defined(WIRING)
       timerAttach(TIMER3OUTCOMPAREA_INT, Timer3Service);  // for Wiring platform only
-#endif
+  #endif
     }
-#endif
+  #endif
 
-#if defined (_useTimer4)
+  #if defined (_useTimer4)
     if(timer == _timer4) {
       TCCR4A = 0;             // normal counting mode
       TCCR4B = _BV(CS41);     // set prescaler of 8
@@ -161,9 +161,9 @@ static void initISR(timer16_Sequence_t timer)
       TIFR4 = _BV(OCF4A);     // clear any pending interrupts;
       TIMSK4 =  _BV(OCIE4A) ; // enable the output compare interrupt
     }
-#endif
+  #endif
 
-#if defined (_useTimer5)
+  #if defined (_useTimer5)
     if(timer == _timer5) {
       TCCR5A = 0;             // normal counting mode
       TCCR5B = _BV(CS51);     // set prescaler of 8
@@ -171,13 +171,13 @@ static void initISR(timer16_Sequence_t timer)
       TIFR5 = _BV(OCF5A);     // clear any pending interrupts;
       TIMSK5 =  _BV(OCIE5A) ; // enable the output compare interrupt
     }
-#endif
-}
+  #endif
+  }
 
-static void finISR(timer16_Sequence_t timer)
-{
+  static void finISR(timer16_Sequence_t timer)
+  {
       //disable use of the given timer
-#if defined WIRING   // Wiring
+  #if defined WIRING   // Wiring
     if(timer == _timer1) {
       #if defined(__AVR_ATmega1281__)||defined(__AVR_ATmega2561__)
       TIMSK1 &=  ~_BV(OCIE1A) ;  // disable timer 1 output compare interrupt
@@ -194,25 +194,25 @@ static void finISR(timer16_Sequence_t timer)
       #endif
       timerDetach(TIMER3OUTCOMPAREA_INT);
     }
-#else
+  #else
       //For arduino - in future: call here to a currently undefined function to reset the timer
-#endif
-}
+  #endif
+  }
 
 
-static boolean isTimerActive(timer16_Sequence_t timer)
-{
+  static boolean isTimerActive(timer16_Sequence_t timer)
+  {
     // returns true if any servo is active on this timer
     for(uint8_t channel=0; channel < SERVOS_PER_TIMER; channel++) {
       if(SERVO(timer,channel).Pin.isActive == true)
         return true;
     }
     return false;
-}
+  }
 
-/****************** end of static functions ******************************/
+  /****************** end of static functions ******************************/
 
-Servo::Servo() {
+  Servo::Servo() {
     if (ServoCount < MAX_SERVOS) {
       this->servoIndex = ServoCount++;                    // assign a servo index to this instance
       servos[this->servoIndex].ticks = usToTicks(DEFAULT_PULSE_WIDTH);   // store default values
@@ -220,13 +220,13 @@ Servo::Servo() {
     else {
       this->servoIndex = INVALID_SERVO;  // too many servos
     }
-}
+  }
 
-uint8_t Servo::attach(int pin) {
+  uint8_t Servo::attach(int pin) {
     return this->attach(pin, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
-}
+  }
 
-uint8_t Servo::attach(int pin, int min, int max) {
+  uint8_t Servo::attach(int pin, int min, int max) {
 
     if (this->servoIndex >= MAX_SERVOS) return -1;
 
@@ -243,22 +243,22 @@ uint8_t Servo::attach(int pin, int min, int max) {
     servos[this->servoIndex].Pin.isActive = true;  // this must be set after the check for isTimerActive
 
     return this->servoIndex;
-}
+  }
 
-void Servo::detach() {
+  void Servo::detach() {
     servos[this->servoIndex].Pin.isActive = false;
     timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex);
     if (!isTimerActive(timer)) finISR(timer);
-}
+  }
 
-void Servo::write(int value) {
+  void Servo::write(int value) {
     if (value < MIN_PULSE_WIDTH) { // treat values less than 544 as angles in degrees (valid values in microseconds are handled as microseconds)
       value = map(constrain(value, 0, 180), 0, 180, SERVO_MIN(), SERVO_MAX());
     }
     this->writeMicroseconds(value);
-}
+  }
 
-void Servo::writeMicroseconds(int value) {
+  void Servo::writeMicroseconds(int value) {
     // calculate and store the values for the given channel
     byte channel = this->servoIndex;
     if (channel < MAX_SERVOS) {  // ensure channel is valid
@@ -267,21 +267,21 @@ void Servo::writeMicroseconds(int value) {
       value = usToTicks(value);  // convert to ticks after compensating for interrupt overhead
 
       CRITICAL_SECTION_START;
-    servo_info[channel].ticks = value;
+      servos[channel].ticks = value;
       CRITICAL_SECTION_END;
     }
-}
+  }
 
-// return the value as degrees
-int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
+  // return the value as degrees
+  int Servo::read() { return map(this->readMicroseconds() + 1, SERVO_MIN(), SERVO_MAX(), 0, 180); }
 
-int Servo::readMicroseconds() {
+  int Servo::readMicroseconds() {
     return (this->servoIndex == INVALID_SERVO) ? 0 : ticksToUs(servos[this->servoIndex].ticks) + TRIM_DURATION;
-}
+  }
 
-bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
+  bool Servo::attached() { return servos[this->servoIndex].Pin.isActive; }
 
-void Servo::move(int value) {
+  void Servo::move(int value) {
     if (this->attach(0) >= 0) {
       this->write(value);
       #if ENABLED(DEACTIVATE_SERVOS_AFTER_MOVE)
@@ -289,6 +289,6 @@ void Servo::move(int value) {
         this->detach();
       #endif
     }
-}
+  }
 
 #endif

MK/module/servo/servo.h

@@ -45,12 +45,14 @@
     detach()    - Stops an attached servos from pulsing its i/o pin. 
  */
 
-#ifndef SERVO_H
-#define SERVO_H
+#ifndef _SERVO_H
+  #define _SERVO_H
 
-#include <inttypes.h>
+  #if HAS(SERVOS)
+    #include <inttypes.h>
+    #include "ServoTimers.h"
 
-/*
+    /*
      * Defines for 16 bit timers used with  Servo library
      *
      * If _useTimerX is defined then TimerX is a 16 bit timer on the current board
@@ -58,32 +60,29 @@
      * _Nbr_16timers indicates how many 16 bit timers are available.
      */
 
-// Architecture specific include
-#include "ServoTimers.h"
-
-#define Servo_VERSION           2     // software version of this library
+    #define Servo_VERSION           2     // software version of this library
 
-#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
-#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
-#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
-#define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
+    #define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
+    #define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
+    #define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached
+    #define REFRESH_INTERVAL    20000     // minumim time to refresh servos in microseconds
 
-#define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
-#define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
+    #define SERVOS_PER_TIMER       12     // the maximum number of servos controlled by one timer
+    #define MAX_SERVOS   (_Nbr_16timers  * SERVOS_PER_TIMER)
 
-#define INVALID_SERVO         255     // flag indicating an invalid servo index
+    #define INVALID_SERVO         255     // flag indicating an invalid servo index
 
-typedef struct {
+    typedef struct {
       uint8_t nbr        :6 ;             // a pin number from 0 to 63
       uint8_t isActive   :1 ;             // true if this channel is enabled, pin not pulsed if false
-} ServoPin_t;
+    } ServoPin_t;
 
-typedef struct {
+    typedef struct {
       ServoPin_t Pin;
       volatile unsigned int ticks;
-} servo_t;
+    } servo_t;
 
-class Servo {
+    class Servo {
       public:
         Servo();
         uint8_t attach(int pin);           // attach the given pin to the next free channel, sets pinMode, returns channel number or 0 if failure
@@ -102,6 +101,7 @@ class Servo {
         uint8_t servoIndex;               // index into the channel data for this servo
         int8_t min;                       // minimum is this value times 4 added to MIN_PULSE_WIDTH
         int8_t max;                       // maximum is this value times 4 added to MAX_PULSE_WIDTH
-};
+    };
 
-#endif
+  #endif // SERVOS
+#endif // _SERVO_H

MK/module/watchdog/watchdog.cpp

 #include "../../base.h"
 
 #if ENABLED(USE_WATCHDOG)
+  #include "watchdog.h"
 
-#include "watchdog.h"
-
-// Initialize watchdog with a 4 sec interrupt time
-void watchdog_init() {
+  // Initialize watchdog with a 4 sec interrupt time
+  void watchdog_init() {
     #if ENABLED(WATCHDOG_RESET_MANUAL)
       // We enable the watchdog timer, but only for the interrupt.
       // Take care, as this requires the correct order of operation, with interrupts disabled. See the datasheet of any AVR chip for details.
@@ -15,19 +14,19 @@ void watchdog_init() {
     #else
       wdt_enable(WDTO_4S);
     #endif
-}
+  }
 
-//===========================================================================
-//=================================== ISR ===================================
-//===========================================================================
+  //===========================================================================
+  //=================================== ISR ===================================
+  //===========================================================================
 
-// Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
-#if ENABLED(WATCHDOG_RESET_MANUAL)
+  // Watchdog timer interrupt, called if main program blocks >1sec and manual reset is enabled.
+  #if ENABLED(WATCHDOG_RESET_MANUAL)
     ISR(WDT_vect) {
       ECHO_LM(ER, "Something is wrong, please turn off the printer.");
       kill(PSTR("ERR:Please Reset")); //kill blocks //16 characters so it fits on a 16x2 display
       while (1); //wait for user or serial reset
     }
-#endif //WATCHDOG_RESET_MANUAL
+  #endif //WATCHDOG_RESET_MANUAL
 
 #endif //USE_WATCHDOG

MK/module/watchdog/watchdog.h

 #ifndef WATCHDOG_H
-#define WATCHDOG_H
+  #define WATCHDOG_H
+  #include <avr/wdt.h>
 
-#include <avr/wdt.h>
-
-// Initialize watchdog with a 4 second interrupt time
-void watchdog_init();
-
-// Reset watchdog. MUST be called at least every 4 seconds after the
-// first watchdog_init or AVR will go into emergency procedures.
-inline void watchdog_reset() { wdt_reset(); }
+  // Initialize watchdog with a 4 second interrupt time
+  void watchdog_init();
 
+  // Reset watchdog. MUST be called at least every 4 seconds after the
+  // first watchdog_init or AVR will go into emergency procedures.
+  inline void watchdog_reset() { wdt_reset(); }
 #endif