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) {
-  Wire.begin();
-  Wire.beginTransmission(0x09);
-  Wire.write('o');                    //to disable ongoing script, only needs to be used once
-  Wire.write('n');
-  Wire.write(red);
-  Wire.write(grn);
-  Wire.write(blu);
-  Wire.endTransmission();
-}
+  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
+    Wire.write('n');
+    Wire.write(red);
+    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.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.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,
-            int jpvt[], double qraux[]);
-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[],
-          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[],
-            double rsd[], int jpvt[], double qraux[]);
-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 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[],
+             double work[], int job);
+  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[],
+              double rsd[], int jpvt[], double qraux[]);
+  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[]);
 
 #endif

MK/module/motion/vector_3.cpp

-/*
-  vector_3.cpp - Vector library for bed leveling
-  Copyright (c) 2012 Lars Brubaker.  All right reserved.
+/**
+ * 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
+ */
 
-  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.
+#include "../../base.h"
 
-  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.
+#if ENABLED(AUTO_BED_LEVELING_FEATURE)
+  #include <math.h>
+  #include "vector_3.h"
 
-  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::vector_3() : x(0), y(0), z(0) { }
 
-#include "../../base.h"
-#include <math.h>
+  vector_3::vector_3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) { }
 
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-#include "vector_3.h"
-
-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::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::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)); }
-
-void vector_3::normalize() {
-  float length = get_length();
-  x /= length;
-  y /= length;
-  z /= length;
-}
-
-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[]) {
-  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) {
-  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) {
-  //row_0.debug("row_0");
-  //row_1.debug("row_1");
-  //row_2.debug("row_2");
-  matrix_3x3 new_matrix;
-  new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
-  new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
-  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() {
-  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) {
-  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();
-
-  // x_row.debug("x_row");
-  // y_row.debug("y_row");
-  // z_row.debug("z_row");
-
-  // create the matrix already correctly transposed
-  matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
-
-  // rot.debug("rot");
-  return rot;
-}
-
-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[]) {
-  ECHO_LT(DB, title);
-  for (uint8_t i = 0; i < 3; i++) {
-    ECHO_S(DB);
-    for (uint8_t j = 0; j < 3; j++) {
-      if (matrix[i][j] >= 0.0) ECHO_C('+');
-      ECHO_V(matrix[i][j], 6);
-      ECHO_C(' ');
+  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::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)); }
+
+  void vector_3::normalize() {
+    float length = get_length();
+    x /= length;
+    y /= length;
+    z /= length;
+  }
+
+  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[]) {
+    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) {
+    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) {
+    //row_0.debug("row_0");
+    //row_1.debug("row_1");
+    //row_2.debug("row_2");
+    matrix_3x3 new_matrix;
+    new_matrix.matrix[0][0] = row_0.x; new_matrix.matrix[0][1] = row_0.y; new_matrix.matrix[0][2] = row_0.z;
+    new_matrix.matrix[1][0] = row_1.x; new_matrix.matrix[1][1] = row_1.y; new_matrix.matrix[1][2] = row_1.z;
+    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() {
+    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) {
+    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();
+
+    // x_row.debug("x_row");
+    // y_row.debug("y_row");
+    // z_row.debug("z_row");
+
+    // create the matrix already correctly transposed
+    matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
+
+    // rot.debug("rot");
+    return rot;
+  }
+
+  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[]) {
+    ECHO_LT(DB, title);
+    for (uint8_t i = 0; i < 3; i++) {
+      ECHO_S(DB);
+      for (uint8_t j = 0; j < 3; j++) {
+        if (matrix[i][j] >= 0.0) ECHO_C('+');
+        ECHO_V(matrix[i][j], 6);
+        ECHO_C(' ');
+      }
+      ECHO_E;
     }
-    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
-
-#if ENABLED(AUTO_BED_LEVELING_FEATURE)
-class matrix_3x3;
-
-struct vector_3 {
-  float x, y, z;
+  #define VECTOR_3_H
 
-  vector_3();
-  vector_3(float x, float y, float z);
+  #if ENABLED(AUTO_BED_LEVELING_FEATURE)
+    class matrix_3x3;
 
-  static vector_3 cross(vector_3 a, vector_3 b);
+    struct vector_3 {
+      float x, y, z;
 
-  vector_3 operator+(vector_3 v);
-  vector_3 operator-(vector_3 v);
-  void normalize();
-  float get_length();
-  vector_3 get_normal();
+      vector_3();
+      vector_3(float x, float y, float z);
 
-  void debug(const char title[]);
+      static vector_3 cross(vector_3 a, vector_3 b);
 
-  void apply_rotation(matrix_3x3 matrix);
-};
+      vector_3 operator+(vector_3 v);
+      vector_3 operator-(vector_3 v);
+      void normalize();
+      float get_length();
+      vector_3 get_normal();
 
-struct matrix_3x3 {
-  float matrix[3][3];
+      void debug(const char title[]);
 
-  static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
-  static matrix_3x3 create_look_at(vector_3 target);
-  static matrix_3x3 transpose(matrix_3x3 original);
+      void apply_rotation(matrix_3x3 matrix);
+    };
 
-  void set_to_identity();
+    struct matrix_3x3 {
+      float matrix[3][3];
 
-  void debug(const char title[]);
-};
+      static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
+      static matrix_3x3 create_look_at(vector_3 target);
+      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.h

@@ -45,63 +45,63 @@
     detach()    - Stops an attached servos from pulsing its i/o pin. 
  */
 
-#ifndef SERVO_H
-#define SERVO_H
-
-#include <inttypes.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
- * timer16_Sequence_t enumerates the sequence that the timers should be allocated
- * _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 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 INVALID_SERVO         255     // flag indicating an invalid servo index
-
-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;
-
-typedef struct {
-  ServoPin_t Pin;
-  volatile unsigned int ticks;
-} servo_t;
-
-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
-    uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
-    void detach();
-    void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
-    void writeMicroseconds(int value); // Write pulse width in microseconds
-    void move(int value);              // attach the servo, then move to value
-                                       // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
-                                       // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DEACTIVATION_DELAY, then detach
-    int read();                        // returns current pulse width as an angle between 0 and 180 degrees
-    int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
-    bool attached();                   // return true if this servo is attached, otherwise false
-
-  private:
-    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
+#ifndef _SERVO_H
+  #define _SERVO_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
+     * timer16_Sequence_t enumerates the sequence that the timers should be allocated
+     * _Nbr_16timers indicates how many 16 bit timers are available.
+     */
+
+    #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 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
+
+    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;
+
+    typedef struct {
+      ServoPin_t Pin;
+      volatile unsigned int ticks;
+    } servo_t;
+
+    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
+        uint8_t attach(int pin, int min, int max); // as above but also sets min and max values for writes.
+        void detach();
+        void write(int value);             // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+        void writeMicroseconds(int value); // Write pulse width in microseconds
+        void move(int value);              // attach the servo, then move to value
+                                           // if value is < 200 it is treated as an angle, otherwise as pulse width in microseconds
+                                           // if DEACTIVATE_SERVOS_AFTER_MOVE wait SERVO_DEACTIVATION_DELAY, then detach
+        int read();                        // returns current pulse width as an angle between 0 and 180 degrees
+        int readMicroseconds();            // returns current pulse width in microseconds for this servo (was read_us() in first release)
+        bool attached();                   // return true if this servo is attached, otherwise false
+
+      private:
+        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 // 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() {
-  #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.
-    wdt_reset();
-    _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
-    _WD_CONTROL_REG = _BV(WDIE) | WDTO_4S;
-  #else
-    wdt_enable(WDTO_4S);
-  #endif
-}
+  // 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.
+      wdt_reset();
+      _WD_CONTROL_REG = _BV(_WD_CHANGE_BIT) | _BV(WDE);
+      _WD_CONTROL_REG = _BV(WDIE) | WDTO_4S;
+    #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)
-  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
+  // 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 //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