diff --git a/CppProperties.json b/CppProperties.json
new file mode 100644
index 0000000..ba79286
--- /dev/null
+++ b/CppProperties.json
@@ -0,0 +1,21 @@
+{
+  "configurations": [
+    {
+      "inheritEnvironments": [
+        "msvc_x86"
+      ],
+      "name": "x86-Release",
+      "includePath": [
+        "${env.INCLUDE}",
+        "${workspaceRoot}\\**"
+      ],
+      "defines": [
+        "WIN32",
+        "NDEBUG",
+        "UNICODE",
+        "_UNICODE"
+      ],
+      "intelliSenseMode": "windows-msvc-x86"
+    }
+  ]
+}
\ No newline at end of file
diff --git a/src/Config.h b/src/Config.h
index d5cb673..0738aba 100644
--- a/src/Config.h
+++ b/src/Config.h
@@ -13,6 +13,8 @@
 
   const int INCOMING_CMD_BUF_SIZE = 100;
 
+  const char SPACE[2] = { ' ', '\0' };
+  const char CRLF[3] = { '\r', '\n', '\0' };
   const char COMM_REPORT_CMD_IDLE[4] = {'R', '0', '0', '\0'};
   const char COMM_REPORT_CMD_START[4] = {'R', '0', '1', '\0'};
   const char COMM_REPORT_CMD_DONE[4] = {'R', '0', '2', '\0'};
diff --git a/src/CurrentState.cpp b/src/CurrentState.cpp
index 9c26735..59b27a7 100644
--- a/src/CurrentState.cpp
+++ b/src/CurrentState.cpp
@@ -136,7 +136,6 @@ String CurrentState::getPosition()
   output += (float)y / (float)stepsPerMmY * 1.0;
   output += " Z";
   output += (float)z / (float)stepsPerMmZ * 1.0;
-  //output += getQAndNewLine();
 
   return output;
 }
diff --git a/src/CurrentState.h b/src/CurrentState.h
index c83d169..a553915 100644
--- a/src/CurrentState.h
+++ b/src/CurrentState.h
@@ -62,9 +62,9 @@ private:
   CurrentState(CurrentState const &);
   void operator=(CurrentState const &);
 
-  long stepsPerMmX;
-  long stepsPerMmY;
-  long stepsPerMmZ;
+  long stepsPerMmX = 1;
+  long stepsPerMmY = 1;
+  long stepsPerMmZ = 1;
 
   int errorCode = 0;
 
diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp
index 144a594..d75968d 100644
--- a/src/F11Handler.cpp
+++ b/src/F11Handler.cpp
@@ -48,13 +48,13 @@ int F11Handler::execute(Command *command)
   {
     switch (stepNr)
     {
-      case 0: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
-      case 1: StepperControl::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
-      case 2: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
-      case 3: StepperControl::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
-      case 4: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
-      case 5: StepperControl::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
-      case 6: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
+      case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
+      case 1: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
+      case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
+      case 3: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
+      case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
+      case 5: Movement::getInstance()->moveToCoords(moveAwayCoord, 0, 0, 0, 0, 0, false, false, false); break;
+      case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false); break;
     }
 
     execution = CurrentState::getInstance()->getLastError();
diff --git a/src/F11Handler.h b/src/F11Handler.h
index 6014c34..8bbfb8d 100644
--- a/src/F11Handler.h
+++ b/src/F11Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F11Handler : public GCodeHandler
 {
diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp
index c9dc624..7c00aa9 100644
--- a/src/F12Handler.cpp
+++ b/src/F12Handler.cpp
@@ -48,13 +48,13 @@ int F12Handler::execute(Command *command)
   {
     switch (stepNr)
     {
-    case 0: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
-    case 1: StepperControl::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
-    case 2: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
-    case 3: StepperControl::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
-    case 4: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
-    case 5: StepperControl::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
-    case 6: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
+    case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
+    case 1: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
+    case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
+    case 3: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
+    case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
+    case 5: Movement::getInstance()->moveToCoords(0, moveAwayCoord, 0, 0, 0, 0, false, false, false); break;
+    case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false); break;
     }
 
     execution = CurrentState::getInstance()->getLastError();
diff --git a/src/F12Handler.h b/src/F12Handler.h
index 31851f9..1e6a983 100644
--- a/src/F12Handler.h
+++ b/src/F12Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F12Handler : public GCodeHandler
 {
diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp
index 764cacc..31055e4 100644
--- a/src/F13Handler.cpp
+++ b/src/F13Handler.cpp
@@ -31,7 +31,7 @@ int F13Handler::execute(Command *command)
     Serial.print("R99 HOME Z\r\n");
   }
 
-  StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
+  Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
 
   int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
   int moveAwayCoord = 10;
@@ -50,13 +50,13 @@ int F13Handler::execute(Command *command)
   {
     switch (stepNr)
     {
-    case 0: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
-    case 1: StepperControl::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
-    case 2: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
-    case 3: StepperControl::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
-    case 4: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
-    case 5: StepperControl::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
-    case 6: StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
+    case 0: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
+    case 1: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
+    case 2: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
+    case 3: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
+    case 4: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
+    case 5: Movement::getInstance()->moveToCoords(0, 0, moveAwayCoord, 0, 0, 0, false, false, false); break;
+    case 6: Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true); break;
     }
 
     execution = CurrentState::getInstance()->getLastError();
diff --git a/src/F13Handler.h b/src/F13Handler.h
index 6b3cd0f..9941247 100644
--- a/src/F13Handler.h
+++ b/src/F13Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F13Handler : public GCodeHandler
 {
diff --git a/src/F14Handler.cpp b/src/F14Handler.cpp
index 5c2e321..86fcc5d 100644
--- a/src/F14Handler.cpp
+++ b/src/F14Handler.cpp
@@ -33,11 +33,11 @@ int F14Handler::execute(Command *command)
     Serial.print("R99 CALIBRATE X\r\n");
   }
 
-  ret = StepperControl::getInstance()->calibrateAxis(0);
+  ret = Movement::getInstance()->calibrateAxis(0);
 
   
 	if (ret == 0) {
-		StepperControl::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false);
+    Movement::getInstance()->moveToCoords(0,0,0, 0,0,0, true, false, false);
 	}
 	
 
diff --git a/src/F14Handler.h b/src/F14Handler.h
index 51916d2..f21563b 100644
--- a/src/F14Handler.h
+++ b/src/F14Handler.h
@@ -13,7 +13,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F14Handler : public GCodeHandler
 {
diff --git a/src/F15Handler.cpp b/src/F15Handler.cpp
index 6622353..f60e514 100644
--- a/src/F15Handler.cpp
+++ b/src/F15Handler.cpp
@@ -32,10 +32,10 @@ int F15Handler::execute(Command *command)
     Serial.print("R99 HOME Z\r\n");
   }
 
-  ret = StepperControl::getInstance()->calibrateAxis(1);
+  ret = Movement::getInstance()->calibrateAxis(1);
 
   if (ret == 0) {
-    StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false);
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false);
   }
 
   if (LOGGING)
diff --git a/src/F15Handler.h b/src/F15Handler.h
index 72b0968..c252009 100644
--- a/src/F15Handler.h
+++ b/src/F15Handler.h
@@ -13,7 +13,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F15Handler : public GCodeHandler
 {
diff --git a/src/F16Handler.cpp b/src/F16Handler.cpp
index df470af..d88f1c3 100644
--- a/src/F16Handler.cpp
+++ b/src/F16Handler.cpp
@@ -32,10 +32,10 @@ int F16Handler::execute(Command *command)
     Serial.print("R99 HOME Z\r\n");
   }
 
-  ret = StepperControl::getInstance()->calibrateAxis(2);
+  ret = Movement::getInstance()->calibrateAxis(2);
 
   if (ret == 0) {
-    StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
   }
 
 
diff --git a/src/F16Handler.h b/src/F16Handler.h
index ac795df..21045ee 100644
--- a/src/F16Handler.h
+++ b/src/F16Handler.h
@@ -13,7 +13,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F16Handler : public GCodeHandler
 {
diff --git a/src/F20Handler.h b/src/F20Handler.h
index 8b724cb..ce283be 100644
--- a/src/F20Handler.h
+++ b/src/F20Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F20Handler : public GCodeHandler
 {
diff --git a/src/F21Handler.h b/src/F21Handler.h
index e0ca468..76fe729 100644
--- a/src/F21Handler.h
+++ b/src/F21Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "ParameterList.h"
 
 class F21Handler : public GCodeHandler
diff --git a/src/F22Handler.h b/src/F22Handler.h
index cf7ab13..4abe0d5 100644
--- a/src/F22Handler.h
+++ b/src/F22Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "ParameterList.h"
 
 class F22Handler : public GCodeHandler
diff --git a/src/F31Handler.h b/src/F31Handler.h
index 90b12d1..1ad93fd 100644
--- a/src/F31Handler.h
+++ b/src/F31Handler.h
@@ -13,7 +13,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "StatusList.h"
 
 class F31Handler : public GCodeHandler
diff --git a/src/F32Handler.h b/src/F32Handler.h
index 1dd7be5..84faf59 100644
--- a/src/F32Handler.h
+++ b/src/F32Handler.h
@@ -13,7 +13,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "StatusList.h"
 
 class F32Handler : public GCodeHandler
diff --git a/src/F81Handler.h b/src/F81Handler.h
index 23275f0..fd60e0f 100644
--- a/src/F81Handler.h
+++ b/src/F81Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 #include "PinControl.h"
 
 class F81Handler : public GCodeHandler
diff --git a/src/F82Handler.h b/src/F82Handler.h
index 2fecb70..924064a 100644
--- a/src/F82Handler.h
+++ b/src/F82Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F82Handler : public GCodeHandler
 {
diff --git a/src/F83Handler.h b/src/F83Handler.h
index 1f8b90c..b08fc11 100644
--- a/src/F83Handler.h
+++ b/src/F83Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F83Handler : public GCodeHandler
 {
diff --git a/src/F84Handler.cpp b/src/F84Handler.cpp
index 1a55602..fee3f2d 100644
--- a/src/F84Handler.cpp
+++ b/src/F84Handler.cpp
@@ -29,19 +29,19 @@ int F84Handler::execute(Command *command)
   if (command->getX() == DO_RESET)
   {
     Serial.print("R99 Will zero X");
-    StepperControl::getInstance()->setPositionX(0);
+    Movement::getInstance()->setPositionX(0);
   }
 
   if (command->getY() == DO_RESET)
   {
     Serial.print("R99 Will zero Y");
-    StepperControl::getInstance()->setPositionY(0);
+    Movement::getInstance()->setPositionY(0);
   }
 
   if (command->getZ() == DO_RESET)
   {
     Serial.print("R99 Will zero Z");
-    StepperControl::getInstance()->setPositionZ(0);
+    Movement::getInstance()->setPositionZ(0);
   }
 
   CurrentState::getInstance()->printQAndNewLine();
diff --git a/src/F84Handler.h b/src/F84Handler.h
index b028997..c4ea774 100644
--- a/src/F84Handler.h
+++ b/src/F84Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class F84Handler : public GCodeHandler
 {
diff --git a/src/G00Handler.cpp b/src/G00Handler.cpp
index 414153e..bba8885 100644
--- a/src/G00Handler.cpp
+++ b/src/G00Handler.cpp
@@ -43,7 +43,7 @@ int G00Handler::execute(Command *command)
   	Serial.print("\r\n");*/
   
 
-  StepperControl::getInstance()->moveToCoords(
+  Movement::getInstance()->moveToCoords(
       command->getX(), command->getY(), command->getZ(),
       command->getA(), command->getB(), command->getC(),
       false, false, false);
diff --git a/src/G00Handler.h b/src/G00Handler.h
index a981ab8..fab5eff 100644
--- a/src/G00Handler.h
+++ b/src/G00Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class G00Handler : public GCodeHandler
 {
diff --git a/src/G28Handler.cpp b/src/G28Handler.cpp
index 14ce2e5..8b50906 100644
--- a/src/G28Handler.cpp
+++ b/src/G28Handler.cpp
@@ -39,9 +39,9 @@ int G28Handler::execute(Command *command)
   currentPoint[0] = sourcePoint[0] / (float)stepsPerMm[0];
   currentPoint[1] = sourcePoint[1] / (float)stepsPerMm[1];
 
-  StepperControl::getInstance()->moveToCoords(currentPoint[0], currentPoint[1], 0, 0, 0, 0, false, false, false);
-  StepperControl::getInstance()->moveToCoords(currentPoint[0], 0, 0, 0, 0, 0, false, false, false);
-  StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, false);
+  Movement::getInstance()->moveToCoords(currentPoint[0], currentPoint[1], 0, 0, 0, 0, false, false, false);
+  Movement::getInstance()->moveToCoords(currentPoint[0], 0, 0, 0, 0, 0, false, false, false);
+  Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, false);
 
   if (LOGGING)
   {
diff --git a/src/G28Handler.h b/src/G28Handler.h
index 831767f..cf48ce5 100644
--- a/src/G28Handler.h
+++ b/src/G28Handler.h
@@ -12,7 +12,7 @@
 #include "CurrentState.h"
 #include "pins.h"
 #include "Config.h"
-#include "StepperControl.h"
+#include "Movement.h"
 
 class G28Handler : public GCodeHandler
 {
diff --git a/src/StepperControl.cpp b/src/Movement.cpp
similarity index 85%
rename from src/StepperControl.cpp
rename to src/Movement.cpp
index 7c8b4de..68c5d92 100644
--- a/src/StepperControl.cpp
+++ b/src/Movement.cpp
@@ -1,19 +1,19 @@
-#include "StepperControl.h"
+#include "Movement.h"
 #include "Debug.h"
 #include "Config.h"
 
-static StepperControl *instance;
+static Movement *instance;
 
-StepperControl *StepperControl::getInstance()
+Movement *Movement::getInstance()
 {
   if (!instance)
   {
-    instance = new StepperControl();
+    instance = new Movement();
   };
   return instance;
 };
 
-void StepperControl::reportEncoders()
+void Movement::reportEncoders()
 {
   Serial.print(COMM_REPORT_ENCODER_SCALED);
   Serial.print(" X");
@@ -35,7 +35,7 @@ void StepperControl::reportEncoders()
 
 }
 
-void StepperControl::getEncoderReport()
+void Movement::getEncoderReport()
 {
   serialBuffer += COMM_REPORT_ENCODER_SCALED;
   serialBuffer += " X";
@@ -57,7 +57,7 @@ void StepperControl::getEncoderReport()
 
 }
 
-void StepperControl::reportStatus(StepperControlAxis *axis, int axisStatus)
+void Movement::reportStatus(MovementAxis *axis, int axisStatus)
 {  
   serialBuffer += COMM_REPORT_CMD_STATUS;
   serialBuffer += " ";
@@ -72,7 +72,7 @@ void StepperControl::reportStatus(StepperControlAxis *axis, int axisStatus)
   //CurrentState::getInstance()->printQAndNewLine();
 }
 
-void StepperControl::reportCalib(StepperControlAxis *axis, int calibStatus)
+void Movement::reportCalib(MovementAxis *axis, int calibStatus)
 {
   Serial.print(COMM_REPORT_CALIB_STATUS);
   Serial.print(" ");
@@ -81,7 +81,7 @@ void StepperControl::reportCalib(StepperControlAxis *axis, int calibStatus)
   CurrentState::getInstance()->printQAndNewLine();
 }
 
-void StepperControl::checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus)
+void Movement::checkAxisSubStatus(MovementAxis *axis, int *axisSubStatus)
 {
   int newStatus = 0;
   bool statusChanged = false;
@@ -121,7 +121,7 @@ void StepperControl::checkAxisSubStatus(StepperControlAxis *axis, int *axisSubSt
 
 //const int MOVEMENT_INTERRUPT_SPEED = 100; // Interrupt cycle in micro seconds
 
-StepperControl::StepperControl()
+Movement::Movement()
 {
 
   // Initialize some variables for testing
@@ -142,9 +142,9 @@ StepperControl::StepperControl()
 
   // Create the axis controllers
 
-  axisX = StepperControlAxis();
-  axisY = StepperControlAxis();
-  axisZ = StepperControlAxis();
+  axisX = MovementAxis();
+  axisY = MovementAxis();
+  axisZ = MovementAxis();
 
   axisX.channelLabel = 'X';
   axisY.channelLabel = 'Y';
@@ -152,9 +152,9 @@ StepperControl::StepperControl()
 
   // Create the encoder controller
 
-  encoderX = StepperControlEncoder();
-  encoderY = StepperControlEncoder();
-  encoderZ = StepperControlEncoder();
+  encoderX = MovementEncoder();
+  encoderY = MovementEncoder();
+  encoderZ = MovementEncoder();
 
   // Load settings
 
@@ -163,9 +163,11 @@ StepperControl::StepperControl()
   motorMotorsEnabled = false;
 }
 
-void StepperControl::loadSettings()
+void Movement::loadSettings()
 {
 
+  /**/ //Serial.println("== load pin numbers ==");
+
   // Load motor settings
 
   axisX.loadPinNumbers(X_STEP_PIN, X_DIR_PIN, X_ENABLE_PIN, X_MIN_PIN, X_MAX_PIN, E_STEP_PIN, E_DIR_PIN, E_ENABLE_PIN);
@@ -176,20 +178,30 @@ void StepperControl::loadSettings()
   axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
   axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;
 
+  /**/ //Serial.println("== load motor settings ==");
+
   loadMotorSettings();
 
   // Load encoder settings
 
+  /**/ //Serial.println("== load encoder settings ==");
+
   loadEncoderSettings();
 
+  /**/ //Serial.println("== load mdl encoder settings ==");
+
   encoderX.loadMdlEncoderId(_MDL_X1);
   encoderY.loadMdlEncoderId(_MDL_Y);
   encoderZ.loadMdlEncoderId(_MDL_Z);
 
+  /**/ //Serial.println("== load encoder pin numbers ==");
+
   encoderX.loadPinNumbers(X_ENCDR_A, X_ENCDR_B, X_ENCDR_A_Q, X_ENCDR_B_Q);
   encoderY.loadPinNumbers(Y_ENCDR_A, Y_ENCDR_B, Y_ENCDR_A_Q, Y_ENCDR_B_Q);
   encoderZ.loadPinNumbers(Z_ENCDR_A, Z_ENCDR_B, Z_ENCDR_A_Q, Z_ENCDR_B_Q);
 
+  /**/ //Serial.println("== load encoder load settings 2 ==");
+
   encoderX.loadSettings(motorConsEncoderType[0], motorConsEncoderScaling[0], motorConsEncoderInvert[0]);
   encoderY.loadSettings(motorConsEncoderType[1], motorConsEncoderScaling[1], motorConsEncoderInvert[1]);
   encoderZ.loadSettings(motorConsEncoderType[2], motorConsEncoderScaling[2], motorConsEncoderInvert[2]);
@@ -197,42 +209,131 @@ void StepperControl::loadSettings()
 }
 
 #if defined(FARMDUINO_EXP_V20)
-  void StepperControl::initTMC2130()
+  void Movement::initTMC2130()
   {
     axisX.initTMC2130();
     axisY.initTMC2130();
     axisZ.initTMC2130();
   }
 
-  void StepperControl::loadSettingsTMC2130()
+  void Movement::loadSettingsTMC2130()
   {
-    int motorCurrent;
-    int stallSensitivity;
-    int microSteps;
+    /**/
+    int motorCurrentX;
+    int stallSensitivityX;
+    int microStepsX;
 
-    motorCurrent = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X);
-    stallSensitivity = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X);
-    microSteps = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X);
-    axisX.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
+    int motorCurrentY;
+    int stallSensitivityY;
+    int microStepsY;
 
-    motorCurrent = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y);
-    stallSensitivity = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y);
-    microSteps = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y);
-    axisY.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
+    int motorCurrentZ;
+    int stallSensitivityZ;
+    int microStepsZ;
+
+    motorCurrentX = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X);
+    stallSensitivityX = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X);
+    microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X);
+    //axisX.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
+
+    motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y);
+    stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y);
+    microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y);
+    //axisY.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
+
+    motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z);
+    stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z);
+    microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z);
+    //axisZ.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
+
+    motorCurrentX = 600;
+    stallSensitivityX = 0;
+    microStepsX = 0;
+
+    motorCurrentY = 600;
+    stallSensitivityY = 0;
+    microStepsY = 0;
+
+    motorCurrentZ = 600;
+    stallSensitivityZ = 0;
+    microStepsZ = 0;
+
+    TMC2130X->push();
+    TMC2130X->toff(3);
+    TMC2130X->tbl(1);
+    TMC2130X->hysteresis_start(4);
+    TMC2130X->hysteresis_end(-2);
+    TMC2130X->rms_current(motorCurrentX); // mA
+    TMC2130X->microsteps(microStepsX);
+    TMC2130X->diag1_stall(1);
+    TMC2130X->diag1_active_high(1);
+    TMC2130X->coolstep_min_speed(0xFFFFF); // 20bit max
+    TMC2130X->THIGH(0);
+    TMC2130X->semin(5);
+    TMC2130X->semax(2);
+    TMC2130X->sedn(0b01);
+    TMC2130X->sg_stall_value(stallSensitivityX);
+
+    TMC2130Y->push();
+    TMC2130Y->toff(3);
+    TMC2130Y->tbl(1);
+    TMC2130Y->hysteresis_start(4);
+    TMC2130Y->hysteresis_end(-2);
+    TMC2130Y->rms_current(motorCurrentY); // mA
+    TMC2130Y->microsteps(microStepsY);
+    TMC2130Y->diag1_stall(1);
+    TMC2130Y->diag1_active_high(1);
+    TMC2130Y->coolstep_min_speed(0xFFFFF); // 20bit max
+    TMC2130Y->THIGH(0);
+    TMC2130Y->semin(5);
+    TMC2130Y->semax(2);
+    TMC2130Y->sedn(0b01);
+    TMC2130Y->sg_stall_value(stallSensitivityY);
+
+    TMC2130Z->push();
+    TMC2130Z->toff(3);
+    TMC2130Z->tbl(1);
+    TMC2130Z->hysteresis_start(4);
+    TMC2130Z->hysteresis_end(-2);
+    TMC2130Z->rms_current(motorCurrentZ); // mA
+    TMC2130Z->microsteps(microStepsZ);
+    TMC2130Z->diag1_stall(1);
+    TMC2130Z->diag1_active_high(1);
+    TMC2130Z->coolstep_min_speed(0xFFFFF); // 20bit max
+    TMC2130Z->THIGH(0);
+    TMC2130Z->semin(5);
+    TMC2130Z->semax(2);
+    TMC2130Z->sedn(0b01);
+    TMC2130Z->sg_stall_value(stallSensitivityZ);
+
+    TMC2130E->push();
+    TMC2130E->toff(3);
+    TMC2130E->tbl(1);
+    TMC2130E->hysteresis_start(4);
+    TMC2130E->hysteresis_end(-2);
+    TMC2130E->rms_current(600); // mA
+    TMC2130E->microsteps(0);
+    TMC2130E->diag1_stall(1);
+    TMC2130E->diag1_active_high(1);
+    TMC2130E->coolstep_min_speed(0xFFFFF); // 20bit max
+    TMC2130E->THIGH(0);
+    TMC2130E->semin(5);
+    TMC2130E->semax(2);
+    TMC2130E->sedn(0b01);
+    TMC2130E->sg_stall_value(0);
 
-    motorCurrent = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z);
-    stallSensitivity = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z);
-    microSteps = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z);
-    axisZ.loadSettingsTMC2130(motorCurrent, stallSensitivity, microSteps);
   }
 
 #endif
 
-void StepperControl::test()
+void Movement::test()
 {
+  axisX.enableMotor();
+
+  //axisY.test();
 
   //axisX.enableMotor();
-  axisX.setMotorStep();
+  //axisX.setMotorStep();
   //delayMicroseconds(10);
   //axisX.setMotorStep();
   //delayMicroseconds(10);
@@ -279,10 +380,10 @@ void StepperControl::test()
   //Serial.print("\r\n");
 }
 
-void StepperControl::test2()
+void Movement::test2()
 {
-  axisX.enableMotor();
-
+  
+  axisX.setMotorStep();
   //CurrentState::getInstance()->printPosition();
   //encoderX.test();
   //encoderY.test();
@@ -296,11 +397,14 @@ void StepperControl::test2()
  * maxStepsPerSecond - maximum number of steps per second
  * maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
  */
-int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled,
+int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled,
                                  unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
                                  bool xHome, bool yHome, bool zHome)
 {
 
+  /**/ //Serial.println("AAA");
+  /**/ //test();
+
   long xDest = xDestScaled * stepsPerMm[0];
   long yDest = yDestScaled * stepsPerMm[1];
   long zDest = zDestScaled * stepsPerMm[2];
@@ -404,6 +508,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
     Serial.print(axisZ.destinationPosition() / stepsPerMm[2]);
     CurrentState::getInstance()->printQAndNewLine();
   }
+
   // Prepare for movement
 
   axisX.movementStarted = false;
@@ -456,6 +561,8 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
 
   emergencyStop = CurrentState::getInstance()->isEmergencyStop();
 
+  unsigned long loopCounts = 0;
+
   // Let the interrupt handle all the movements
   while ((axisActive[0] || axisActive[1] || axisActive[2]) && !emergencyStop)
   {
@@ -463,6 +570,27 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
     checkEncoders();
     #endif
 
+    /**/
+    if (loopCounts % 1000 == 0)
+    {
+
+      //Serial.print("R99");
+      //Serial.print(" missed step ");
+      //Serial.print(motorConsMissedSteps[1]);
+      //Serial.print(" axis pos ");
+      //Serial.print(axisY.currentPosition());
+      //Serial.print("\r\n");
+
+      //Serial.print("X - ");
+      //axisX.test();
+
+      //Serial.print("Y - ");
+      //axisY.test();
+
+    }
+    loopCounts++;
+    
+
     checkAxisSubStatus(&axisX, &axisSubStep[0]);
     checkAxisSubStatus(&axisY, &axisSubStep[1]);
     checkAxisSubStatus(&axisZ, &axisSubStep[2]);
@@ -474,21 +602,21 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
     if (axisX.isStepDone())
     {
       axisX.checkMovement();
-      checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
+      //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
       axisX.resetStepDone();
     }
 
     if (axisY.isStepDone())
     {
       axisY.checkMovement();
-      checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]);
+      //checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]);
       axisY.resetStepDone();
     }
 
     if (axisZ.isStepDone())
     {
       axisZ.checkMovement();
-      checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]);
+      //checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]);
       axisZ.resetStepDone();
     }
 
@@ -645,6 +773,10 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
 
     if (serialMessageDelay > 300 && serialBuffer.length() == 0 && serialBufferSending == 0)
     {
+
+      //Serial.print("Y-");
+      //axisY.test();/**/
+
       serialMessageDelay = 0;
 
       switch(serialMessageNr)
@@ -662,8 +794,8 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
           break;
 
         case 2:
-
           #if defined(FARMDUINO_EXP_V20)
+          
           serialBuffer += "R89";
           serialBuffer += " X";
           serialBuffer += axisX.getLoad();
@@ -672,9 +804,12 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
           serialBuffer += " Z";
           serialBuffer += axisZ.getLoad();
           serialBuffer += CurrentState::getInstance()->getQAndNewLine();
+          
           #endif
           break;
 
+        default:
+          break;
       }
 
       serialMessageNr++;
@@ -790,7 +925,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
   return error;
 }
 
-void StepperControl::serialBufferEmpty()
+void Movement::serialBufferEmpty()
 {
   while (serialBuffer.length() > 0)
   {
@@ -798,7 +933,7 @@ void StepperControl::serialBufferEmpty()
   }
 }
 
-void StepperControl::serialBufferSendNext()
+void Movement::serialBufferSendNext()
 {
   // Send the next char in the serialBuffer
   if (serialBuffer.length() > 0)
@@ -834,7 +969,7 @@ void StepperControl::serialBufferSendNext()
 // Calibration
 //
 
-int StepperControl::calibrateAxis(int axis)
+int Movement::calibrateAxis(int axis)
 {
 
   // Load motor and encoder settings
@@ -869,8 +1004,8 @@ int StepperControl::calibrateAxis(int axis)
   reportEndStops();
 
   // Select the right axis
-  StepperControlAxis *calibAxis;
-  StepperControlEncoder *calibEncoder;
+  MovementAxis *calibAxis;
+  MovementEncoder *calibEncoder;
 
   switch (axis)
   {
@@ -1238,7 +1373,7 @@ int StepperControl::calibrateAxis(int axis)
 int debugPrintCount = 0;
 
 // Check encoder to verify the motor is at the right position
-void StepperControl::checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled)
+void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled)
 {
 #if !defined(FARMDUINO_EXP_V20)
   if (*encoderEnabled)
@@ -1310,6 +1445,16 @@ void StepperControl::checkAxisVsEncoder(StepperControlAxis *axis, StepperControl
 
 #if defined(FARMDUINO_EXP_V20)
 
+  /**/
+  /*
+  Serial.print("R99");
+  Serial.print(" XXX ");
+  Serial.print(" cur pos ");
+  Serial.print(axis->currentPosition());
+  Serial.print(" last pos ");
+  Serial.print(*lastPosition);
+  */
+
   if (*encoderEnabled) {
     if (axis->stallDetected()) {
       // In case of stall detection, count this as a missed step
@@ -1326,11 +1471,19 @@ void StepperControl::checkAxisVsEncoder(StepperControlAxis *axis, StepperControl
       encoder->setPosition(axis->currentPosition());
     }
   }
+
+  //Serial.print(" new last pos ");
+  //Serial.print(*lastPosition);
+  //Serial.print(" en pos ");
+  //Serial.print(encoder->currentPosition());
+  //Serial.print("\r\n");
+
+
 #endif
 
 }
 
-void StepperControl::loadMotorSettings()
+void Movement::loadMotorSettings()
 {
 
   // Load settings
@@ -1424,14 +1577,17 @@ void StepperControl::loadMotorSettings()
   axisY.loadMotorSettings(speedMax[1], speedMin[1], speedHome[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], endStInv2[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1], motorStopAtHome[1], motorMaxSize[1], motorStopAtMax[1]);
   axisZ.loadMotorSettings(speedMax[2], speedMin[2], speedHome[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], endStInv2[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2], motorStopAtHome[2], motorMaxSize[2], motorStopAtMax[2]);
 
+  /**/
+
+/*
 #if defined(FARMDUINO_EXP_V20)
   loadSettingsTMC2130();
 #endif
-
+*/
   primeMotors();
 }
 
-bool StepperControl::intToBool(int value)
+bool Movement::intToBool(int value)
 {
   if (value == 1)
   {
@@ -1440,7 +1596,7 @@ bool StepperControl::intToBool(int value)
   return false;
 }
 
-void StepperControl::loadEncoderSettings()
+void Movement::loadEncoderSettings()
 {
 
   // Load encoder settings
@@ -1505,7 +1661,7 @@ void StepperControl::loadEncoderSettings()
   }
 }
 
-unsigned long StepperControl::getMaxLength(unsigned long lengths[3])
+unsigned long Movement::getMaxLength(unsigned long lengths[3])
 {
   unsigned long max = lengths[0];
   for (int i = 1; i < 3; i++)
@@ -1518,7 +1674,7 @@ unsigned long StepperControl::getMaxLength(unsigned long lengths[3])
   return max;
 }
 
-void StepperControl::enableMotors()
+void Movement::enableMotors()
 {
   motorMotorsEnabled = true;
 
@@ -1529,7 +1685,7 @@ void StepperControl::enableMotors()
   delay(100);
 }
 
-void StepperControl::disableMotorsEmergency()
+void Movement::disableMotorsEmergency()
 {
   motorMotorsEnabled = false;
 
@@ -1538,7 +1694,7 @@ void StepperControl::disableMotorsEmergency()
   axisZ.disableMotor();
 }
 
-void StepperControl::disableMotors()
+void Movement::disableMotors()
 {
   motorMotorsEnabled = false;
 
@@ -1549,19 +1705,19 @@ void StepperControl::disableMotors()
   delay(100);
 }
 
-void StepperControl::primeMotors()
+void Movement::primeMotors()
 {
   if (motorKeepActive[0] == 1) { axisX.enableMotor(); } else { axisX.disableMotor(); }
   if (motorKeepActive[1] == 1) { axisY.enableMotor(); } else { axisY.disableMotor(); }
   if (motorKeepActive[2] == 1) { axisZ.enableMotor(); } else { axisZ.disableMotor(); }
 }
 
-bool StepperControl::motorsEnabled()
+bool Movement::motorsEnabled()
 {
   return motorMotorsEnabled;
 }
 
-bool StepperControl::endStopsReached()
+bool Movement::endStopsReached()
 {
 
   if (axisX.endStopsReached() ||
@@ -1573,7 +1729,7 @@ bool StepperControl::endStopsReached()
   return false;
 }
 
-void StepperControl::storePosition()
+void Movement::storePosition()
 {
 
 #if !defined(FARMDUINO_EXP_V20)
@@ -1613,41 +1769,41 @@ void StepperControl::storePosition()
 
 }
 
-void StepperControl::reportEndStops()
+void Movement::reportEndStops()
 {
   CurrentState::getInstance()->printEndStops();
 }
 
-void StepperControl::reportPosition()
+void Movement::reportPosition()
 {
   CurrentState::getInstance()->printPosition();
 }
 
-void StepperControl::storeEndStops()
+void Movement::storeEndStops()
 {
   CurrentState::getInstance()->storeEndStops();
 }
 
-void StepperControl::setPositionX(long pos)
+void Movement::setPositionX(long pos)
 {
   axisX.setCurrentPosition(pos);
   encoderX.setPosition(pos);
 }
 
-void StepperControl::setPositionY(long pos)
+void Movement::setPositionY(long pos)
 {
   axisY.setCurrentPosition(pos);
   encoderY.setPosition(pos);
 }
 
-void StepperControl::setPositionZ(long pos)
+void Movement::setPositionZ(long pos)
 {
   axisZ.setCurrentPosition(pos);
   encoderZ.setPosition(pos);
 }
 
 // Handle movement by checking each axis
-void StepperControl::handleMovementInterrupt(void)
+void Movement::handleMovementInterrupt(void)
 {
   // No need to check the encoders for Farmduino 1.4
   #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
@@ -1662,7 +1818,7 @@ void StepperControl::handleMovementInterrupt(void)
 
 }
 
-void StepperControl::checkEncoders()
+void Movement::checkEncoders()
 {
   // read encoder pins using the arduino IN registers instead of digital in
   // because it used much fewer cpu cycles
diff --git a/src/StepperControl.h b/src/Movement.h
similarity index 77%
rename from src/StepperControl.h
rename to src/Movement.h
index a7b073b..abcf608 100644
--- a/src/StepperControl.h
+++ b/src/Movement.h
@@ -1,32 +1,32 @@
 /*
- * StepperControl.h
+ * Movement.h
  *
  *  Created on: 16 maj 2014
  *      Author: MattLech
  */
 
-#ifndef STEPPERCONTROL_H_
-#define STEPPERCONTROL_H_
+#ifndef MOVEMENT_H_
+#define MOVEMENT_H_
 
 #include "Arduino.h"
 #include "CurrentState.h"
 #include "ParameterList.h"
-#include "StepperControlAxis.h"
-#include "StepperControlEncoder.h"
+#include "MovementAxis.h"
+#include "MovementEncoder.h"
 #include "pins.h"
 #include "Config.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include "Command.h"
 
-class StepperControl
+class Movement
 {
 public:
-  StepperControl();
-  StepperControl(StepperControl const &);
-  void operator=(StepperControl const &);
+  Movement();
+  Movement(Movement const &);
+  void operator=(Movement const &);
 
-  static StepperControl *getInstance();
+  static Movement *getInstance();
   //int moveAbsolute(	long xDest, long yDest,long zDest,
   //			unsigned int maxStepsPerSecond,
   //			unsigned int maxAccelerationStepsPerSecond);
@@ -70,13 +70,13 @@ public:
 
 private:
 
-  StepperControlAxis axisX;
-  StepperControlAxis axisY;
-  StepperControlAxis axisZ;
+  MovementAxis axisX;
+  MovementAxis axisY;
+  MovementAxis axisZ;
 
-  StepperControlEncoder encoderX;
-  StepperControlEncoder encoderY;
-  StepperControlEncoder encoderZ;
+  MovementEncoder encoderX;
+  MovementEncoder encoderY;
+  MovementEncoder encoderZ;
 
   //char serialBuffer[100];
   String serialBuffer;
@@ -88,8 +88,8 @@ private:
   void serialBufferSendNext();
   void serialBufferEmpty();
 
-  void checkAxisVsEncoder(StepperControlAxis *axis, StepperControlEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled);
-  void checkAxisSubStatus(StepperControlAxis *axis, int *axisSubStatus);
+  void checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, float *missedSteps, long *lastPosition, long *encoderLastPosition, int *encoderUseForPos, float *encoderStepDecay, bool *encoderEnabled);
+  void checkAxisSubStatus(MovementAxis *axis, int *axisSubStatus);
 
   bool axisActive[3] = { false, false, false };
   int axisSubStep[3] = { 0, 0, 0 };
@@ -103,8 +103,8 @@ private:
 
   void storeEndStops();
   void reportEndStops();
-  void reportStatus(StepperControlAxis *axis, int axisSubStatus);
-  void reportCalib(StepperControlAxis *axis, int calibStatus);
+  void reportStatus(MovementAxis *axis, int axisSubStatus);
+  void reportCalib(MovementAxis *axis, int calibStatus);
 
   unsigned long getMaxLength(unsigned long lengths[3]);
   bool endStopsReached();
@@ -147,4 +147,4 @@ private:
 
 };
 
-#endif /* STEPPERCONTROL_H_ */
+#endif /* MOVEMENT_H_ */
diff --git a/src/StepperControlAxis.cpp b/src/MovementAxis.cpp
similarity index 70%
rename from src/StepperControlAxis.cpp
rename to src/MovementAxis.cpp
index fceb325..b838060 100644
--- a/src/StepperControlAxis.cpp
+++ b/src/MovementAxis.cpp
@@ -1,14 +1,22 @@
-#include "StepperControlAxis.h"
+/*
+ * MovementAxis.cpp
+ *
+ *  Created on: 18 juli 2015
+ *      Author: Tim Evers
+ */
 
+#include "MovementAxis.h"
+
+/*
 #if defined(FARMDUINO_EXP_V20)
   static TMC2130Stepper TMC2130X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
   static TMC2130Stepper TMC2130Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
   static TMC2130Stepper TMC2130Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
   static TMC2130Stepper TMC2130E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
 #endif
+*/
 
-
-StepperControlAxis::StepperControlAxis()
+MovementAxis::MovementAxis()
 {
   lastCalcLog = 0;
 
@@ -41,59 +49,80 @@ StepperControlAxis::StepperControlAxis()
 
   stepIsOn = false;
 
-  setMotorStepWrite = &StepperControlAxis::setMotorStepWriteDefault;
-  setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteDefault2;
-  resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteDefault;
-  resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteDefault2;
+  setMotorStepWrite = &MovementAxis::setMotorStepWriteDefault;
+  setMotorStepWrite2 = &MovementAxis::setMotorStepWriteDefault2;
+  resetMotorStepWrite = &MovementAxis::resetMotorStepWriteDefault;
+  resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteDefault2;
 
 }
 
-void StepperControlAxis::test()
+void MovementAxis::test()
 {
 
+    Serial.print("Check timing ");
+    Serial.print(" motorInterruptSpeed ");
+    Serial.print(motorInterruptSpeed);
+    Serial.print(" axisSpeed ");
+    Serial.print(axisSpeed);
+    Serial.print(" active ");
+    Serial.print(axisActive);
+    Serial.print(" move ticks ");
+    Serial.print(moveTicks);
+    Serial.print(" step on ");
+    Serial.print(stepIsOn);
+    Serial.print(" tick on ");
+    Serial.print(stepOnTick);
+    Serial.print(" tick off ");
+    Serial.print(stepOffTick);
+    Serial.print(" mov step done");
+    Serial.print(movementStepDone);
+    Serial.print("\r\n");
+
 }
 
 #if defined(FARMDUINO_EXP_V20)
 
-unsigned int StepperControlAxis::getLostSteps()
+unsigned int MovementAxis::getLostSteps()
 {
   return TMC2130A->LOST_STEPS();
 }
 
-void StepperControlAxis::initTMC2130()
+void MovementAxis::initTMC2130()
 {
+
   if (channelLabel == 'X')
   {
-    TMC2130A = &TMC2130X;
-    TMC2130B = &TMC2130E;
+    TMC2130A = TMC2130X;
+    TMC2130B = TMC2130E;
   }
   if (channelLabel == 'Y')
   {
-    TMC2130A = &TMC2130Y;
+    TMC2130A = TMC2130Y;
   }
   if (channelLabel == 'Z')
   {
-    TMC2130A = &TMC2130Z;
+    TMC2130A = TMC2130Z;
   }
 
-  TMC2130A->begin();                      // Initiate pins and registeries
+  //TMC2130A->begin();                      // Initiate pins and registeries
   //TMC2130A->shaft_dir(0);                 // Set direction
 
   if (channelLabel == 'X')
   {
-    TMC2130B->begin();                    // Initiate pins and registeries
+    //TMC2130B->begin();                    // Initiate pins and registeries
     //TMC2130B->shaft_dir(0);               // Set direction
   }
 
-  setMotorStepWrite = &StepperControlAxis::setMotorStepWriteTMC2130;
-  setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteTMC2130_2;
-  resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteTMC2130;
-  resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteTMC2130_2;
+  setMotorStepWrite = &MovementAxis::setMotorStepWriteTMC2130;
+  setMotorStepWrite2 = &MovementAxis::setMotorStepWriteTMC2130_2;
+  resetMotorStepWrite = &MovementAxis::resetMotorStepWriteTMC2130;
+  resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteTMC2130_2;
 
 }
 
-void StepperControlAxis::loadSettingsTMC2130(int motorCurrent, int  stallSensitivity, int microSteps)
+void MovementAxis::loadSettingsTMC2130(int motorCurrent, int  stallSensitivity, int microSteps)
 {
+  /**/
   /*
   Serial.println("loading settings");
 
@@ -118,45 +147,105 @@ void StepperControlAxis::loadSettingsTMC2130(int motorCurrent, int  stallSensiti
   Serial.println(" = ");
   */
 
+/*
   TMC2130A->rms_current(motorCurrent);    // Set the required current in mA  
   TMC2130A->microsteps(microSteps);       // Minimum of micro steps needed
   TMC2130A->chm(true);                    // Set the chopper mode to classic const. off time
   TMC2130A->diag1_stall(1);               // Activate stall diagnostics
   TMC2130A->sgt(stallSensitivity);        // Set stall detection sensitivity. most -64 to +64 least
-  TMC2130A->shaft_dir(0);                 // Set direction
+  //TMC2130A->shaft_dir(0);                 // Set direction
+*/
+
+  /*
+  TMC2130A->push();
+  TMC2130A->toff(3);
+  TMC2130A->tbl(1);
+  TMC2130A->hysteresis_start(4);
+  TMC2130A->hysteresis_end(-2);
+  TMC2130A->rms_current(motorCurrent); // mA
+  TMC2130A->microsteps(microSteps);
+  TMC2130A->diag1_stall(1);
+  TMC2130A->diag1_active_high(1);
+  TMC2130A->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130A->THIGH(0);
+  TMC2130A->semin(5);
+  TMC2130A->semax(2);
+  TMC2130A->sedn(0b01);
+  TMC2130A->sg_stall_value(stallSensitivity);
+
+
+  /*
+  TMC2130A->push();
+  TMC2130A->toff(3);
+  TMC2130A->tbl(1);
+  TMC2130A->hysteresis_start(4);
+  TMC2130A->hysteresis_end(-2);
+  TMC2130A->rms_current(600); // mA
+  TMC2130A->microsteps(16);
+  TMC2130A->diag1_stall(1);
+  TMC2130A->diag1_active_high(1);
+  TMC2130A->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130A->THIGH(0);
+  TMC2130A->semin(5);
+  TMC2130A->semax(2);
+  TMC2130A->sedn(0b01);
+  TMC2130A->sg_stall_value(0);
+  */
 
   if (channelLabel == 'X')
   {
+    /*
     TMC2130B->rms_current(motorCurrent);   // Set the required current in mA  
     TMC2130B->microsteps(microSteps);      // Minimum of micro steps needed
     TMC2130B->chm(true);                   // Set the chopper mode to classic const. off time
     TMC2130B->diag1_stall(1);              // Activate stall diagnostics
     TMC2130B->sgt(stallSensitivity);       // Set stall detection sensitivity. most -64 to +64 least
     TMC2130B->shaft_dir(0);                // Set direction
+    */
+
+    /*
+    TMC2130B->push();
+    TMC2130B->toff(3);
+    TMC2130B->tbl(1);
+    TMC2130B->hysteresis_start(4);
+    TMC2130B->hysteresis_end(-2);
+    TMC2130B->rms_current(motorCurrent); // mA
+    TMC2130B->microsteps(microSteps);
+    TMC2130B->diag1_stall(1);
+    TMC2130B->diag1_active_high(1);
+    TMC2130B->coolstep_min_speed(0xFFFFF); // 20bit max
+    TMC2130B->THIGH(0);
+    TMC2130B->semin(5);
+    TMC2130B->semax(2);
+    TMC2130B->sedn(0b01);
+    TMC2130B->sg_stall_value(stallSensitivity);
+    */
   }
 }
 
-bool StepperControlAxis::stallDetected() {
+bool MovementAxis::stallDetected() {
   return TMC2130A->stallguard();
 }
 
-uint16_t StepperControlAxis::getLoad() {
-  return TMC2130A->sg_result();
+uint16_t MovementAxis::getLoad() {
+  //return TMC2130A->sg_result();
+  /**/ 
+  return 0;
 }
 
 #endif
 
-long StepperControlAxis::getLastPosition()
+long MovementAxis::getLastPosition()
 {
   return axisLastPosition;
 }
 
-void StepperControlAxis::setLastPosition(long position)
+void MovementAxis::setLastPosition(long position)
 {
   axisLastPosition = position;
 }
 
-unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec)
+unsigned int MovementAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec)
 {
 
   int newSpeed = 0;
@@ -279,7 +368,7 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
   return newSpeed;
 }
 
-void StepperControlAxis::checkAxisDirection()
+void MovementAxis::checkAxisDirection()
 {
 
   if (coordHomeAxis)
@@ -302,7 +391,7 @@ void StepperControlAxis::checkAxisDirection()
   }
 }
 
-void StepperControlAxis::setDirectionAxis()
+void MovementAxis::setDirectionAxis()
 {
 
   if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)))
@@ -315,7 +404,7 @@ void StepperControlAxis::setDirectionAxis()
   }
 }
 
-void StepperControlAxis::checkMovement()
+void MovementAxis::checkMovement()
 {
 
   checkAxisDirection();
@@ -358,7 +447,7 @@ void StepperControlAxis::checkMovement()
   }
 }
 
-void StepperControlAxis::incrementTick()
+void MovementAxis::incrementTick()
 {
   if (axisActive)
   {
@@ -366,17 +455,17 @@ void StepperControlAxis::incrementTick()
   }
 }
 
-void StepperControlAxis::checkTiming()
+void MovementAxis::checkTiming()
 {
-
   if (stepIsOn)
   {
     if (moveTicks >= stepOffTick)
     {
-
       // Negative flank for the steps
       resetMotorStep();
       setTicks();
+
+      //test();
     }
   }
   else
@@ -385,15 +474,16 @@ void StepperControlAxis::checkTiming()
     {
       if (moveTicks >= stepOnTick)
       {
-
         // Positive flank for the steps
         setStepAxis();
+
+        //test();
       }
     }
   }
 }
 
-void StepperControlAxis::setTicks()
+void MovementAxis::setTicks()
 {
   // Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
   // This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
@@ -401,7 +491,7 @@ void StepperControlAxis::setTicks()
   stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed);
 }
 
-void StepperControlAxis::setStepAxis()
+void MovementAxis::setStepAxis()
 {
 
   stepIsOn = true;
@@ -419,7 +509,7 @@ void StepperControlAxis::setStepAxis()
   setMotorStep();
 }
 
-bool StepperControlAxis::endStopAxisReached(bool movement_forward)
+bool MovementAxis::endStopAxisReached(bool movement_forward)
 {
 
   bool min_endstop = false;
@@ -455,7 +545,7 @@ bool StepperControlAxis::endStopAxisReached(bool movement_forward)
   return 0;
 }
 
-void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2)
+void MovementAxis::MovementAxis::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2)
 {
   pinStep = step;
   pinDirection = dir;
@@ -469,7 +559,7 @@ void StepperControlAxis::StepperControlAxis::loadPinNumbers(int step, int dir, i
   pinMax = max;
 }
 
-void StepperControlAxis::loadMotorSettings(
+void MovementAxis::loadMotorSettings(
     long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv,
     bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl,
     bool stopAtHome, long maxSize, bool stopAtMax)
@@ -494,32 +584,32 @@ void StepperControlAxis::loadMotorSettings(
 
   if (pinStep == 54)
   {
-    setMotorStepWrite = &StepperControlAxis::setMotorStepWrite54;
-    resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite54;
+    setMotorStepWrite = &MovementAxis::setMotorStepWrite54;
+    resetMotorStepWrite = &MovementAxis::resetMotorStepWrite54;
   }
   
   if (pinStep == 60)
   {
-    setMotorStepWrite = &StepperControlAxis::setMotorStepWrite60;
-    resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite60;
+    setMotorStepWrite = &MovementAxis::setMotorStepWrite60;
+    resetMotorStepWrite = &MovementAxis::resetMotorStepWrite60;
   }
   
 
   if (pinStep == 46)
   {
-    setMotorStepWrite = &StepperControlAxis::setMotorStepWrite46;
-    resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite46;
+    setMotorStepWrite = &MovementAxis::setMotorStepWrite46;
+    resetMotorStepWrite = &MovementAxis::resetMotorStepWrite46;
   }
 
   if (pin2Step == 26)
   {
-    setMotorStepWrite2 = &StepperControlAxis::setMotorStepWrite26;
-    resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWrite26;
+    setMotorStepWrite2 = &MovementAxis::setMotorStepWrite26;
+    resetMotorStepWrite2 = &MovementAxis::resetMotorStepWrite26;
   }
 
 }
 
-bool StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home)
+bool MovementAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home)
 {
 
   coordSourcePoint = sourcePoint;
@@ -571,7 +661,7 @@ bool StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint
   return changed;
 }
 
-void StepperControlAxis::enableMotor()
+void MovementAxis::enableMotor()
 {
   digitalWrite(pinEnable, LOW);
   if (motorMotor2Enl)
@@ -581,7 +671,7 @@ void StepperControlAxis::enableMotor()
   movementMotorActive = true;
 }
 
-void StepperControlAxis::disableMotor()
+void MovementAxis::disableMotor()
 {
   digitalWrite(pinEnable, HIGH);
   if (motorMotor2Enl)
@@ -591,10 +681,10 @@ void StepperControlAxis::disableMotor()
   movementMotorActive = false;
 }
 
-void StepperControlAxis::setDirectionUp()
+void MovementAxis::setDirectionUp()
 {
 
-#if !defined(FARMDUINO_EXP_V20)
+//#if !defined(FARMDUINO_EXP_V20)
   if (motorMotorInv)
   {
     digitalWrite(pinDirection, LOW);
@@ -612,8 +702,9 @@ void StepperControlAxis::setDirectionUp()
   {
     digitalWrite(pin2Direction, HIGH);
   }
-#endif
+//#endif
 
+/*
 #if defined(FARMDUINO_EXP_V20)
 
   // The TMC2130 uses a command to change direction, not a pin
@@ -636,12 +727,12 @@ void StepperControlAxis::setDirectionUp()
   }
 
 #endif
-
+*/
 }
 
-void StepperControlAxis::setDirectionDown()
+void MovementAxis::setDirectionDown()
 {
-  #if !defined(FARMDUINO_EXP_V20)
+  //#if !defined(FARMDUINO_EXP_V20)
 
   if (motorMotorInv)
   {
@@ -661,8 +752,8 @@ void StepperControlAxis::setDirectionDown()
     digitalWrite(pin2Direction, LOW);
   }
 
-  #endif
-
+  //#endif
+/*
   #if defined(FARMDUINO_EXP_V20)
 
   // The TMC2130 uses a command to change direction, not a pin
@@ -685,20 +776,20 @@ void StepperControlAxis::setDirectionDown()
   }
 
   #endif
-
+*/
 }
 
-void StepperControlAxis::setMovementUp()
+void MovementAxis::setMovementUp()
 {
   movementUp = true;
 }
 
-void StepperControlAxis::setMovementDown()
+void MovementAxis::setMovementDown()
 {
   movementUp = false;
 }
 
-void StepperControlAxis::setDirectionHome()
+void MovementAxis::setDirectionHome()
 {
   if (motorHomeIsUp)
   {
@@ -712,7 +803,7 @@ void StepperControlAxis::setDirectionHome()
   }
 }
 
-void StepperControlAxis::setDirectionAway()
+void MovementAxis::setDirectionAway()
 {
   if (motorHomeIsUp)
   {
@@ -726,7 +817,7 @@ void StepperControlAxis::setDirectionAway()
   }
 }
 
-unsigned long StepperControlAxis::getLength(long l1, long l2)
+unsigned long MovementAxis::getLength(long l1, long l2)
 {
   if (l1 > l2)
   {
@@ -738,34 +829,34 @@ unsigned long StepperControlAxis::getLength(long l1, long l2)
   }
 }
 
-bool StepperControlAxis::endStopsReached()
+bool MovementAxis::endStopsReached()
 {
   return ((digitalRead(pinMin) == motorEndStopInv2) || (digitalRead(pinMax) == motorEndStopInv2)) && motorEndStopEnbl;
 }
 
-bool StepperControlAxis::endStopMin()
+bool MovementAxis::endStopMin()
 {
   //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
   return ((digitalRead(pinMin) == motorEndStopInv2) && motorEndStopEnbl);
 }
 
-bool StepperControlAxis::endStopMax()
+bool MovementAxis::endStopMax()
 {
   //return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
   return ((digitalRead(pinMax) == motorEndStopInv2) && motorEndStopEnbl);
 }
 
-bool StepperControlAxis::isAxisActive()
+bool MovementAxis::isAxisActive()
 {
   return axisActive;
 }
 
-void StepperControlAxis::deactivateAxis()
+void MovementAxis::deactivateAxis()
 {
   axisActive = false;
 }
 
-void StepperControlAxis::setMotorStep()
+void MovementAxis::setMotorStep()
 {
   stepIsOn = true;
 
@@ -779,7 +870,7 @@ void StepperControlAxis::setMotorStep()
   }
 }
 
-void StepperControlAxis::resetMotorStep()
+void MovementAxis::resetMotorStep()
 {
   stepIsOn = false;
   movementStepDone = true;
@@ -794,77 +885,77 @@ void StepperControlAxis::resetMotorStep()
   }
 }
 
-bool StepperControlAxis::pointReached(long currentPoint, long destinationPoint)
+bool MovementAxis::pointReached(long currentPoint, long destinationPoint)
 {
   return (destinationPoint == currentPoint);
 }
 
-long StepperControlAxis::currentPosition()
+long MovementAxis::currentPosition()
 {
   return coordCurrentPoint;
 }
 
-void StepperControlAxis::setCurrentPosition(long newPos)
+void MovementAxis::setCurrentPosition(long newPos)
 {
   coordCurrentPoint = newPos;
 }
 
-long StepperControlAxis::destinationPosition()
+long MovementAxis::destinationPosition()
 {
   return coordDestinationPoint;
 }
 
-void StepperControlAxis::setMaxSpeed(long speed)
+void MovementAxis::setMaxSpeed(long speed)
 {
   motorSpeedMax = speed;
 }
 
-void StepperControlAxis::activateDebugPrint()
+void MovementAxis::activateDebugPrint()
 {
   debugPrint = true;
 }
 
-bool StepperControlAxis::isStepDone()
+bool MovementAxis::isStepDone()
 {
   return movementStepDone;
 }
 
-void StepperControlAxis::resetStepDone()
+void MovementAxis::resetStepDone()
 {
   movementStepDone = false;
 }
 
-bool StepperControlAxis::movingToHome()
+bool MovementAxis::movingToHome()
 {
   return movementToHome;
 }
 
-bool StepperControlAxis::movingUp()
+bool MovementAxis::movingUp()
 {
   return movementUp;
 }
 
-bool StepperControlAxis::isAccelerating()
+bool MovementAxis::isAccelerating()
 {
   return movementAccelerating;
 }
 
-bool StepperControlAxis::isDecelerating()
+bool MovementAxis::isDecelerating()
 {
   return movementDecelerating;
 }
 
-bool StepperControlAxis::isCruising()
+bool MovementAxis::isCruising()
 {
   return movementCruising;
 }
 
-bool StepperControlAxis::isCrawling()
+bool MovementAxis::isCrawling()
 {
   return movementCrawling;
 }
 
-bool StepperControlAxis::isMotorActive()
+bool MovementAxis::isMotorActive()
 {
   return movementMotorActive;
 }
@@ -872,34 +963,34 @@ bool StepperControlAxis::isMotorActive()
 /// Functions for pin writing using alternative method
 
 // Pin write default functions
-void StepperControlAxis::setMotorStepWriteDefault()
+void MovementAxis::setMotorStepWriteDefault()
 {
   digitalWrite(pinStep, HIGH);
 }
 
-void StepperControlAxis::setMotorStepWriteDefault2()
+void MovementAxis::setMotorStepWriteDefault2()
 {
   digitalWrite(pin2Step, HIGH);
 }
 
-void StepperControlAxis::resetMotorStepWriteDefault()
+void MovementAxis::resetMotorStepWriteDefault()
 {
   digitalWrite(pinStep, LOW);
 }
 
-void StepperControlAxis::resetMotorStepWriteDefault2()
+void MovementAxis::resetMotorStepWriteDefault2()
 {
   digitalWrite(pin2Step, LOW);
 }
 
 // X step
-void StepperControlAxis::setMotorStepWrite54()
+void MovementAxis::setMotorStepWrite54()
 {
   //PF0
   PORTF |= B00000001;
 }
 
-void StepperControlAxis::resetMotorStepWrite54()
+void MovementAxis::resetMotorStepWrite54()
 {
   //PF0
   PORTF &= B11111110;
@@ -907,38 +998,38 @@ void StepperControlAxis::resetMotorStepWrite54()
 
 
 // X step 2
-void StepperControlAxis::setMotorStepWrite26()
+void MovementAxis::setMotorStepWrite26()
 {
   //PA4
   PORTA |= B00010000;
 }
 
-void StepperControlAxis::resetMotorStepWrite26()
+void MovementAxis::resetMotorStepWrite26()
 {
   PORTA &= B11101111;
 }
 
 // Y step
-void StepperControlAxis::setMotorStepWrite60()
+void MovementAxis::setMotorStepWrite60()
 {
   //PF6
   PORTF |= B01000000;
 }
 
-void StepperControlAxis::resetMotorStepWrite60()
+void MovementAxis::resetMotorStepWrite60()
 {
   //PF6
   PORTF &= B10111111;
 }
 
 // Z step
-void StepperControlAxis::setMotorStepWrite46()
+void MovementAxis::setMotorStepWrite46()
 {
   //PL3
   PORTL |= B00001000;
 }
 
-void StepperControlAxis::resetMotorStepWrite46()
+void MovementAxis::resetMotorStepWrite46()
 {
   //PL3
   PORTL &= B11110111;
@@ -947,17 +1038,17 @@ void StepperControlAxis::resetMotorStepWrite46()
 #if defined(FARMDUINO_EXP_V20)
 //// TMC2130 Functions
 
-void StepperControlAxis::setMotorStepWriteTMC2130()
+void MovementAxis::setMotorStepWriteTMC2130()
 {
   // TMC2130 works on each edge of the step pulse, 
   // so instead of setting the step bit, 
   // toggle the bit here
-
+  
   if (tmcStep)
   {
     digitalWrite(pinStep, HIGH);
     tmcStep = false;
-    }
+  }
   else
   {
     digitalWrite(pinStep, LOW);
@@ -965,7 +1056,7 @@ void StepperControlAxis::setMotorStepWriteTMC2130()
   }
 }
 
-void StepperControlAxis::setMotorStepWriteTMC2130_2()
+void MovementAxis::setMotorStepWriteTMC2130_2()
 {
   if (tmcStep2)
   {
@@ -979,12 +1070,12 @@ void StepperControlAxis::setMotorStepWriteTMC2130_2()
   }
 }
 
-void StepperControlAxis::resetMotorStepWriteTMC2130()
+void MovementAxis::resetMotorStepWriteTMC2130()
 {
   // No action needed
 }
 
-void StepperControlAxis::resetMotorStepWriteTMC2130_2()
+void MovementAxis::resetMotorStepWriteTMC2130_2()
 {
   // No action needed
 }
diff --git a/src/StepperControlAxis.h b/src/MovementAxis.h
similarity index 62%
rename from src/StepperControlAxis.h
rename to src/MovementAxis.h
index 3b72733..dd128dd 100644
--- a/src/StepperControlAxis.h
+++ b/src/MovementAxis.h
@@ -1,32 +1,32 @@
 /*
- * StepperControlAxis.h
+ * MovementAxis.h
  *
  *  Created on: 18 juli 2015
  *      Author: Tim Evers
  */
 
-#ifndef STEPPERCONTROLAXIS_H_
-#define STEPPERCONTROLAXIS_H_
+#ifndef MovementAXIS_H_
+#define MovementAXIS_H_
 
 #include "Arduino.h"
 #include "CurrentState.h"
 #include "ParameterList.h"
 #include "pins.h"
-#include "StepperControlEncoder.h"
+#include "MovementEncoder.h"
 #include "Config.h"
 #include <stdio.h>
 #include <stdlib.h>
 
 #if defined(FARMDUINO_EXP_V20)
-  #include <TMC2130Stepper.h>
+  #include "TMC2130.h"
 #endif
 
 
-class StepperControlAxis
+class MovementAxis
 {
 
 public:
-  StepperControlAxis();
+  MovementAxis();
 
 #if defined(FARMDUINO_EXP_V20)
   TMC2130Stepper *TMC2130A;
@@ -134,47 +134,48 @@ private:
   bool motorEndStopEnbl; // enable the use of the end stops
 
   // motor settings
-  long motorSpeedMax;       // maximum speed in steps per second
-  long motorSpeedMin;       // minimum speed in steps per second
-  long motorSpeedHome;      // homing speed in steps per second
-  long motorStepsAcc;       // number of steps used for acceleration
-  long motorTimeOut;        // timeout in seconds
-  bool motorHomeIsUp;       // direction to move when reached 0 on axis but no end stop detected while homing
-  bool motorMotorInv;       // invert motor direction
-  bool motorMotor2Enl;      // enable secondary motor
-  bool motorMotor2Inv;      // invert secondary motor direction
-  long motorInterruptSpeed; // period of interrupt in micro seconds
-  bool motorStopAtHome;     // stop at home position or also use other side of the axis
-  long motorMaxSize;        // maximum size of the axis in steps
-  bool motorStopAtMax;      // stop at the maximum size
+  long motorSpeedMax        = 300;    // maximum speed in steps per second
+  long motorSpeedMin        = 50;     // minimum speed in steps per second
+  long motorSpeedHome       = 50;     // homing speed in steps per second
+  long motorStepsAcc        = 300;    // number of steps used for acceleration
+  long motorTimeOut         = 120;    // timeout in seconds
+  bool motorHomeIsUp        = false;  // direction to move when reached 0 on axis but no end stop detected while homing
+  bool motorMotorInv        = false;  // invert motor direction
+  bool motorMotor2Enl       = false;  // enable secondary motor
+  bool motorMotor2Inv       = false;  // invert secondary motor direction
+  long motorInterruptSpeed  = 100;    // period of interrupt in micro seconds
+  bool motorStopAtHome      = false;  // stop at home position or also use other side of the axis
+  long motorMaxSize         = 0;      // maximum size of the axis in steps
+  bool motorStopAtMax       = false;  // stop at the maximum size
 
   // coordinates
-  long coordSourcePoint; // all coordinated in steps
-  long coordCurrentPoint;
-  long coordDestinationPoint;
-  bool coordHomeAxis; // homing command
+  long coordSourcePoint     = 0;      // all coordinated in steps
+  long coordCurrentPoint    = 0;
+  long coordDestinationPoint = 0;
+  bool coordHomeAxis        = false; // homing command
 
-  long axisLastPosition = 0;
+  long axisLastPosition     = 0;
 
   // movement handling
-  unsigned long movementLength;
-  unsigned long maxLenth;
-  unsigned long moveTicks;
-  unsigned long stepOnTick;
-  unsigned long stepOffTick;
-  unsigned long axisSpeed;
+  unsigned long movementLength  = 0;
+  unsigned long maxLenth        = 0;
+  unsigned long moveTicks       = 0;
+  unsigned long stepOnTick      = 0;
+  unsigned long stepOffTick     = 0;
+  unsigned long axisSpeed       = 0;
+
+  bool axisActive           = false;
+  bool movementUp           = false;
+  bool movementToHome       = false;
+  bool movementStepDone     = false;
+  bool movementAccelerating = false;
+  bool movementDecelerating = false;
+  bool movementCruising     = false;
+  bool movementCrawling     = false;
+  bool movementMotorActive  = false;
+  bool movementMoving       = false;
+  bool stepIsOn             = false;
 
-  bool axisActive;
-  bool movementUp;
-  bool movementToHome;
-  bool movementStepDone;
-  bool movementAccelerating;
-  bool movementDecelerating;
-  bool movementCruising;
-  bool movementCrawling;
-  bool movementMotorActive;
-  bool movementMoving;
-  bool stepIsOn;
   void step(long &currentPoint, unsigned long steps, long destinationPoint);
   bool pointReached(long currentPoint, long destinationPoint);
   unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec);
@@ -182,10 +183,10 @@ private:
   void checkAxisDirection();
 
 
-  void (StepperControlAxis::*setMotorStepWrite)();
-  void (StepperControlAxis::*setMotorStepWrite2)();
-  void (StepperControlAxis::*resetMotorStepWrite)();
-  void (StepperControlAxis::*resetMotorStepWrite2)();
+  void (MovementAxis::*setMotorStepWrite)();
+  void (MovementAxis::*setMotorStepWrite2)();
+  void (MovementAxis::*resetMotorStepWrite)();
+  void (MovementAxis::*resetMotorStepWrite2)();
 
   void setMotorStepWriteDefault();
   void setMotorStepWriteDefault2();
@@ -202,4 +203,4 @@ private:
 
 };
 
-#endif /* STEPPERCONTROLAXIS_H_ */
+#endif /* MovementAXIS_H_ */
diff --git a/src/StepperControlEncoder.cpp b/src/MovementEncoder.cpp
similarity index 85%
rename from src/StepperControlEncoder.cpp
rename to src/MovementEncoder.cpp
index c9e7f44..c8f776f 100644
--- a/src/StepperControlEncoder.cpp
+++ b/src/MovementEncoder.cpp
@@ -1,6 +1,6 @@
-#include "StepperControlEncoder.h"
+#include "MovementEncoder.h"
 
-StepperControlEncoder::StepperControlEncoder()
+MovementEncoder::MovementEncoder()
 {
   //lastCalcLog	= 0;
 
@@ -24,7 +24,7 @@ StepperControlEncoder::StepperControlEncoder()
   mdlEncoder = _MDL_X1;
 }
 
-void StepperControlEncoder::test()
+void MovementEncoder::test()
 {
   /*
                 Serial.print("R88 ");
@@ -42,7 +42,7 @@ void StepperControlEncoder::test()
 */
 }
 
-void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ)
+void MovementEncoder::loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ)
 {
   pinChannelA = channelA;
   pinChannelB = channelB;
@@ -53,7 +53,7 @@ void StepperControlEncoder::loadPinNumbers(int channelA, int channelB, int chann
   shiftChannels();
 }
 
-void StepperControlEncoder::loadSettings(int encType, long scaling, int invert)
+void MovementEncoder::loadSettings(int encType, long scaling, int invert)
 {
   encoderType = encType;
   scalingFactor = scaling;
@@ -69,12 +69,12 @@ void StepperControlEncoder::loadSettings(int encType, long scaling, int invert)
 //  encoderType = 0; // TEVE 2017-04-20 Disabling the differential channels. They take too much time to read.
 }
 
-void StepperControlEncoder::loadMdlEncoderId(MdlSpiEncoders encoder)
+void MovementEncoder::loadMdlEncoderId(MdlSpiEncoders encoder)
 {
   mdlEncoder = encoder;
 }
 
-void StepperControlEncoder::setPosition(long newPosition)
+void MovementEncoder::setPosition(long newPosition)
 {
   #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
     position = newPosition;
@@ -94,7 +94,7 @@ void StepperControlEncoder::setPosition(long newPosition)
   #endif
 }
 
-long StepperControlEncoder::currentPosition()
+long MovementEncoder::currentPosition()
 {
 
 
@@ -114,12 +114,12 @@ long StepperControlEncoder::currentPosition()
   }
 }
 
-long StepperControlEncoder::currentPositionRaw()
+long MovementEncoder::currentPositionRaw()
 {
     return position * encoderInvert;
 }
 
-void StepperControlEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
+void MovementEncoder::checkEncoder(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
 {
   #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
     shiftChannels();
@@ -149,7 +149,7 @@ rotation ----------------------------------------------------->
 
 */
 
-void StepperControlEncoder::processEncoder()
+void MovementEncoder::processEncoder()
 {
 
   #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
@@ -190,7 +190,7 @@ void StepperControlEncoder::processEncoder()
 
 }
 
-void StepperControlEncoder::readChannels()
+void MovementEncoder::readChannels()
 {
 
 #if defined(RAMPS_V14) || defined(FARMDUINO_V10)
@@ -223,7 +223,7 @@ void StepperControlEncoder::readChannels()
 
 }
 
-void StepperControlEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
+void MovementEncoder::setChannels(bool channelA, bool channelB, bool channelAQ, bool channelBQ)
 {
   // read the new values from the coder
 
@@ -245,7 +245,7 @@ void StepperControlEncoder::setChannels(bool channelA, bool channelB, bool chann
   }
 }
 
-void StepperControlEncoder::shiftChannels()
+void MovementEncoder::shiftChannels()
 {
 
   // Save the current enoder status to later on compare with new values
@@ -255,27 +255,27 @@ void StepperControlEncoder::shiftChannels()
 }
 
 
-void StepperControlEncoder::setEnable(bool enable)
+void MovementEncoder::setEnable(bool enable)
 {
   encoderEnabled = enable;
 }
 
-void StepperControlEncoder::setStepDecay(float stepDecay)
+void MovementEncoder::setStepDecay(float stepDecay)
 {
   encoderStepDecay = stepDecay;
 }
 
-void StepperControlEncoder::setMovementDirection(bool up)
+void MovementEncoder::setMovementDirection(bool up)
 {
   encoderMovementUp = up;
 }
 
-float StepperControlEncoder::getMissedSteps()
+float MovementEncoder::getMissedSteps()
 {
   return missedSteps;
 }
 
-void StepperControlEncoder::checkMissedSteps()
+void MovementEncoder::checkMissedSteps()
 {
   #if !defined(FARMDUINO_EXP_V20)
     if (encoderEnabled)
diff --git a/src/StepperControlEncoder.h b/src/MovementEncoder.h
similarity index 58%
rename from src/StepperControlEncoder.h
rename to src/MovementEncoder.h
index 71db396..b932277 100644
--- a/src/StepperControlEncoder.h
+++ b/src/MovementEncoder.h
@@ -1,12 +1,12 @@
 /*
- * StepperControlEncoder.h
+ * MovementEncoder.h
  *
  *  Created on: 29 okt 2015
  *      Author: Tim Evers
  */
 
-#ifndef STEPPERCONTROLENCODER_H_
-#define STEPPERCONTROLENCODER_H_
+#ifndef MovementENCODER_H_
+#define MovementENCODER_H_
 
 #include "Arduino.h"
 //#include "CurrentState.h"
@@ -16,13 +16,13 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <SPI.h>
-#include "StepperControlAxis.h"
+#include "MovementAxis.h"
 
-class StepperControlEncoder
+class MovementEncoder
 {
 
 public:
-  StepperControlEncoder();
+  MovementEncoder();
 
   void loadPinNumbers(int channelA, int channelB, int channelAQ, int channelBQ);
   void loadSettings(int encType, long scaling, int invert);
@@ -50,38 +50,38 @@ public:
 
 private:
   // pin settings
-  int pinChannelA;
-  int pinChannelAQ;
-  int pinChannelB;
-  int pinChannelBQ;
+  int pinChannelA = 0;
+  int pinChannelAQ = 0;
+  int pinChannelB = 0;
+  int pinChannelBQ = 0;
 
   // channels
-  bool prvValChannelA;
-  bool prvValChannelB;
-  bool curValChannelA;
-  bool curValChannelB;
+  bool prvValChannelA = false;
+  bool prvValChannelB = false;
+  bool curValChannelA = false;
+  bool curValChannelB = false;
 
-  bool readChannelA;
-  bool readChannelAQ;
-  bool readChannelB;
-  bool readChannelBQ;
+  bool readChannelA = false;
+  bool readChannelAQ = false;
+  bool readChannelB = false;
+  bool readChannelBQ = false;
 
   // encoder
-  long position;
-  long scalingFactor;
-  float floatScalingFactor;
-  int encoderType;
-  int encoderInvert;
+  long position = 0;
+  long scalingFactor = 0;
+  float floatScalingFactor = 0;
+  int encoderType = 0;
+  int encoderInvert = 0;
 
 
-  float missedSteps;
-  long encoderLastPosition;
-  float encoderStepDecay;
-  bool encoderEnabled;
-  bool encoderMovementUp;
+  float missedSteps = 0;
+  long encoderLastPosition = 0;
+  float encoderStepDecay = 0;
+  bool encoderEnabled = false;
+  bool encoderMovementUp = false;
 
   MdlSpiEncoders mdlEncoder = _MDL_X1;
 
 };
 
-#endif /* STEPPERCONTROLENCODER_H_ */
+#endif /* MovementENCODER_H_ */
diff --git a/src/README.md b/src/README.md
deleted file mode 100644
index b6dc710..0000000
--- a/src/README.md
+++ /dev/null
@@ -1,7 +0,0 @@
-farmbot-arduino-controller
-==========================
-This software is responsible for receiving G-Codes from the Raspberry Pi, execute them and report back the results.
-
-Technicals
-==========================
-Created with eclipseArduino V2 - For more details see http://www.baeyens.it/eclipse/
diff --git a/src/TMC2130.cpp b/src/TMC2130.cpp
new file mode 100644
index 0000000..1bb18f7
--- /dev/null
+++ b/src/TMC2130.cpp
@@ -0,0 +1,74 @@
+/*
+ * TMC2130.cpp
+ *
+ *  Created on: 03 nov 2019
+ *      Author: Tim Evers
+ */
+
+#include "TMC2130.h"
+
+/*
+static TMC2130Stepper *TMC2130X;
+static TMC2130Stepper *TMC2130Y;
+static TMC2130Stepper *TMC2130Z;
+static TMC2130Stepper *TMC2130E;
+*/
+
+/*
+static TMC2130Stepper TMC2130_X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
+static TMC2130Stepper TMC2130_Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
+static TMC2130Stepper TMC2130_Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
+static TMC2130Stepper TMC2130_E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
+*/
+
+/*
+static TMC2130Holder *instance;
+
+TMC2130Holder *TMC2130Holder::getInstance()
+{
+  if (!instance)
+  {
+    instance = new TMC2130Holder();
+  };
+  return instance;
+};
+
+TMC2130Holder::TMC2130Holder()
+{
+}
+
+void TMC2130Holder::loadDrivers()
+{
+  TMC2130Stepper TMC2130X = new TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
+  TMC2130Stepper TMC2130Y = new TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
+  TMC2130Stepper TMC2130Z = new TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
+  TMC2130Stepper TMC2130E = new TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
+
+  tmcTMC2130X = &TMC2130X;
+  tmcTMC2130Y = &TMC2130Y;
+  tmcTMC2130Z = &TMC2130Z;
+  tmcTMC2130E = &TMC2130E;
+
+}
+
+
+TMC2130Stepper* TMC2130Holder::TMC2130X()
+{
+  return tmcTMC2130X;
+}
+
+TMC2130Stepper* TMC2130Holder::TMC2130Y()
+{
+  return tmcTMC2130Y;
+}
+
+TMC2130Stepper* TMC2130Holder::TMC2130Z()
+{
+  return tmcTMC2130Z;
+}
+c
+TMC2130Stepper* TMC2130Holder::TMC2130E()
+{
+  return tmcTMC213EZ;
+}
+*/
\ No newline at end of file
diff --git a/src/TMC2130.h b/src/TMC2130.h
new file mode 100644
index 0000000..84b82d5
--- /dev/null
+++ b/src/TMC2130.h
@@ -0,0 +1,78 @@
+/*
+ * TMC2130.h
+ *
+ *  Created on: 03 nov 2019
+ *      Author: Tim Evers
+ */
+
+
+#ifndef TMC2130_H_
+#define TMC2130_H_
+
+//#include <TMC2130Stepper.h>
+//#include <TMC2130Stepper_REGDEFS.h>
+
+#include "TMC2130/TMC2130Stepper.h"
+#include "TMC2130/TMC2130Stepper_REGDEFS.h"
+
+#include "pins.h"
+
+static TMC2130Stepper *TMC2130X;
+static TMC2130Stepper *TMC2130Y;
+static TMC2130Stepper *TMC2130Z;
+static TMC2130Stepper *TMC2130E;
+
+
+//static TMC2130Stepper TMC2130_X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
+//static TMC2130Stepper TMC2130_Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
+//static TMC2130Stepper TMC2130_Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
+//static TMC2130Stepper TMC2130_E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
+
+//static TMC2130Stepper TMC2130X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
+//static TMC2130Stepper TMC2130Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
+//static TMC2130Stepper TMC2130Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
+//static TMC2130Stepper TMC2130E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
+
+#endif /* TMC2130_H_ */
+
+
+
+/*
+
+#if defined(FARMDUINO_EXP_V20)
+  #include <TMC2130Stepper.h>
+  //#include "TMC2130Stepper.h"
+#endif
+
+#ifndef TMC2130Holder_H_
+#define TMC2130Holder_H_
+
+class TMC2130Holder
+{
+public:
+  TMC2130Holder();
+  TMC2130Holder(TMC2130Holder const &);
+  void operator=(TMC2130Holder const &);
+
+  static TMC2130Holder *getInstance();
+
+  void loadDrivers();
+
+  TMC2130Stepper* TMC2130X();
+  TMC2130Stepper* TMC2130Y();
+  TMC2130Stepper* TMC2130Z();
+  TMC2130Stepper* TMC2130E();
+
+private:
+
+  static TMC2130Stepper *tmcTMC2130X;
+  static TMC2130Stepper *tmcTMC2130Y;
+  static TMC2130Stepper *tmcTMC2130Z;
+  static TMC2130Stepper *tmcTMC2130E;
+
+};
+
+#endif 
+*/
+
+/* TMC2130Holder_H_ */
diff --git a/src/TMC2130_Backup/SW_SPI.cpp b/src/TMC2130/SPI/SW_SPI.cpp
similarity index 100%
rename from src/TMC2130_Backup/SW_SPI.cpp
rename to src/TMC2130/SPI/SW_SPI.cpp
diff --git a/src/TMC2130_Backup/SW_SPI.h b/src/TMC2130/SPI/SW_SPI.h
similarity index 100%
rename from src/TMC2130_Backup/SW_SPI.h
rename to src/TMC2130/SPI/SW_SPI.h
diff --git a/src/TMC2130_Backup/TMC2130Stepper.cpp b/src/TMC2130/TMC2130Stepper.cpp
similarity index 82%
rename from src/TMC2130_Backup/TMC2130Stepper.cpp
rename to src/TMC2130/TMC2130Stepper.cpp
index a461f66..81fe7bc 100644
--- a/src/TMC2130_Backup/TMC2130Stepper.cpp
+++ b/src/TMC2130/TMC2130Stepper.cpp
@@ -1,26 +1,16 @@
 #include "TMC2130Stepper.h"
 #include "TMC2130Stepper_MACROS.h"
 #include <SPI.h>
-#include "SW_SPI.h"
 
-TMC2130Stepper::TMC2130Stepper(uint16_t pinCS) : _pinCS(pinCS), uses_sw_spi(false) {}
+TMC2130Stepper::TMC2130Stepper(uint16_t pinCS) : _pinCS(pinCS) {}
 
 TMC2130Stepper::TMC2130Stepper(uint16_t pinEN, uint16_t pinDIR, uint16_t pinStep, uint16_t pinCS) :
 	_pinEN(pinEN),
 	_pinSTEP(pinStep),
 	_pinCS(pinCS),
-	_pinDIR(pinDIR),
-	uses_sw_spi(false)
+	_pinDIR(pinDIR)
 	{}
 
-TMC2130Stepper::TMC2130Stepper(uint16_t pinEN, uint16_t pinDIR, uint16_t pinStep, uint16_t pinCS, uint16_t pinMOSI, uint16_t pinMISO, uint16_t pinSCK) :
-	_pinEN(pinEN),
-	_pinSTEP(pinStep),
-	_pinCS(pinCS),
-	_pinDIR(pinDIR),
-	uses_sw_spi(true)
-	{ TMC_SW_SPI.setPins(pinMOSI, pinMISO, pinSCK); }
-
 void TMC2130Stepper::begin() {
 #ifdef TMC2130DEBUG
 	Serial.println("TMC2130 Stepper driver library");
@@ -51,8 +41,6 @@ void TMC2130Stepper::begin() {
 	pinMode(_pinCS, OUTPUT);
 	digitalWrite(_pinCS, HIGH);
 
-	if (uses_sw_spi) TMC_SW_SPI.init();
-
 	// Reset registers
 	push();
 
@@ -61,68 +49,39 @@ void TMC2130Stepper::begin() {
 }
 
 void TMC2130Stepper::send2130(uint8_t addressByte, uint32_t *config) {
-	if (uses_sw_spi) {
+
+  SPI.begin();
+	SPI.beginTransaction(SPISettings(16000000/8, MSBFIRST, SPI_MODE3));
+	digitalWrite(_pinCS, LOW);
+
+	status_response = SPI.transfer(addressByte & 0xFF); // s =
+
+	if (addressByte >> 7) { // Check if WRITE command
+		//*config &= ~mask; // Clear bits being set
+		//*config |= (value & mask); // Set new values
+		SPI.transfer((*config >> 24) & 0xFF);
+		SPI.transfer((*config >> 16) & 0xFF);
+		SPI.transfer((*config >>  8) & 0xFF);
+		SPI.transfer(*config & 0xFF);
+	} else { // READ command
+		SPI.transfer16(0x0000); // Clear SPI
+		SPI.transfer16(0x0000);
+		digitalWrite(_pinCS, HIGH);
 		digitalWrite(_pinCS, LOW);
 
-		status_response = TMC_SW_SPI.transfer(addressByte & 0xFF); // s =
-
-		if (addressByte >> 7) { // Check if WRITE command
-			//*config &= ~mask; // Clear bits being set
-			//*config |= (value & mask); // Set new values
-			TMC_SW_SPI.transfer((*config >> 24) & 0xFF);
-			TMC_SW_SPI.transfer((*config >> 16) & 0xFF);
-			TMC_SW_SPI.transfer((*config >>  8) & 0xFF);
-			TMC_SW_SPI.transfer(*config & 0xFF);
-		} else { // READ command
-			TMC_SW_SPI.transfer16(0x0000); // Clear SPI
-			TMC_SW_SPI.transfer16(0x0000);
-			digitalWrite(_pinCS, HIGH);
-			digitalWrite(_pinCS, LOW);
-
-			TMC_SW_SPI.transfer(addressByte & 0xFF); // Send the address byte again
-			*config = TMC_SW_SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= TMC_SW_SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= TMC_SW_SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= TMC_SW_SPI.transfer(0x00);
-		}
-
-		digitalWrite(_pinCS, HIGH);
-	} else {
-		SPI.begin();
-		SPI.beginTransaction(SPISettings(16000000/8, MSBFIRST, SPI_MODE3));
-		digitalWrite(_pinCS, LOW);
-
-		status_response = SPI.transfer(addressByte & 0xFF); // s =
-
-		if (addressByte >> 7) { // Check if WRITE command
-			//*config &= ~mask; // Clear bits being set
-			//*config |= (value & mask); // Set new values
-			SPI.transfer((*config >> 24) & 0xFF);
-			SPI.transfer((*config >> 16) & 0xFF);
-			SPI.transfer((*config >>  8) & 0xFF);
-			SPI.transfer(*config & 0xFF);
-		} else { // READ command
-			SPI.transfer16(0x0000); // Clear SPI
-			SPI.transfer16(0x0000);
-			digitalWrite(_pinCS, HIGH);
-			digitalWrite(_pinCS, LOW);
-
-			SPI.transfer(addressByte & 0xFF); // Send the address byte again
-			*config = SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= SPI.transfer(0x00);
-			*config <<= 8;
-			*config|= SPI.transfer(0x00);
-		}
-
-		digitalWrite(_pinCS, HIGH);
-		SPI.endTransaction();
+		SPI.transfer(addressByte & 0xFF); // Send the address byte again
+		*config = SPI.transfer(0x00);
+		*config <<= 8;
+		*config|= SPI.transfer(0x00);
+		*config <<= 8;
+		*config|= SPI.transfer(0x00);
+		*config <<= 8;
+		*config|= SPI.transfer(0x00);
 	}
+
+	digitalWrite(_pinCS, HIGH);
+	SPI.endTransaction();
+
 }
 
 bool TMC2130Stepper::checkOT() {
diff --git a/src/TMC2130_Backup/TMC2130Stepper.h b/src/TMC2130/TMC2130Stepper.h
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper.h
rename to src/TMC2130/TMC2130Stepper.h
diff --git a/src/TMC2130_Backup/TMC2130Stepper_CHOPCONF.cpp b/src/TMC2130/TMC2130Stepper_CHOPCONF.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_CHOPCONF.cpp
rename to src/TMC2130/TMC2130Stepper_CHOPCONF.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_COOLCONF.cpp b/src/TMC2130/TMC2130Stepper_COOLCONF.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_COOLCONF.cpp
rename to src/TMC2130/TMC2130Stepper_COOLCONF.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_DRV_STATUS.cpp b/src/TMC2130/TMC2130Stepper_DRV_STATUS.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_DRV_STATUS.cpp
rename to src/TMC2130/TMC2130Stepper_DRV_STATUS.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_GCONF.cpp b/src/TMC2130/TMC2130Stepper_GCONF.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_GCONF.cpp
rename to src/TMC2130/TMC2130Stepper_GCONF.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_IHOLD_IRUN.cpp b/src/TMC2130/TMC2130Stepper_IHOLD_IRUN.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_IHOLD_IRUN.cpp
rename to src/TMC2130/TMC2130Stepper_IHOLD_IRUN.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_MACROS.h b/src/TMC2130/TMC2130Stepper_MACROS.h
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_MACROS.h
rename to src/TMC2130/TMC2130Stepper_MACROS.h
diff --git a/src/TMC2130_Backup/TMC2130Stepper_PWMCONF.cpp b/src/TMC2130/TMC2130Stepper_PWMCONF.cpp
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_PWMCONF.cpp
rename to src/TMC2130/TMC2130Stepper_PWMCONF.cpp
diff --git a/src/TMC2130_Backup/TMC2130Stepper_REGDEFS.h b/src/TMC2130/TMC2130Stepper_REGDEFS.h
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_REGDEFS.h
rename to src/TMC2130/TMC2130Stepper_REGDEFS.h
diff --git a/src/TMC2130_Backup/TMC2130Stepper_UTILITY.h b/src/TMC2130/TMC2130Stepper_UTILITY.h
similarity index 100%
rename from src/TMC2130_Backup/TMC2130Stepper_UTILITY.h
rename to src/TMC2130/TMC2130Stepper_UTILITY.h
diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp
index 7843307..8261754 100644
--- a/src/farmbot_arduino_controller.cpp
+++ b/src/farmbot_arduino_controller.cpp
@@ -1,14 +1,5 @@
 // Do not remove the include below
 #include "farmbot_arduino_controller.h"
-#include "pins.h"
-#include "Config.h"
-#include "StepperControl.h"
-#include "ServoControl.h"
-#include "PinGuard.h"
-#include "MemoryFree.h"
-#include "Debug.h"
-#include "CurrentState.h"
-#include <SPI.h>
 
 #if !defined(FARMDUINO_EXP_V20)
 #include "TimerOne.h"
@@ -18,6 +9,12 @@ bool stepperInit = false;
 bool stepperFlip = false;
 
 static char commandEndChar = 0x0A;
+char commandChar[INCOMING_CMD_BUF_SIZE + 1];
+//String commandString = "";
+char incomingChar = 0;
+char incomingCommandArray[INCOMING_CMD_BUF_SIZE];
+int incomingCommandPointer = 0;
+
 static GCodeProcessor *gCodeProcessor = new GCodeProcessor();
 
 int reportingPeriod = 5000;
@@ -31,17 +28,13 @@ unsigned long pinGuardCurrentTime;
 
 int lastParamChangeNr = 0;
 
-String commandString = "";
-char incomingChar = 0;
-char incomingCommandArray[INCOMING_CMD_BUF_SIZE];
-int incomingCommandPointer = 0;
-
 // Blink led routine used for testing
 bool blink = false;
 void blinkLed()
 {
   blink = !blink;
-  digitalWrite(LED_PIN, blink);
+  //digitalWrite(LED_PIN, blink);
+  digitalWrite(13, blink);
 }
 
 // Interrupt handling for:
@@ -70,355 +63,30 @@ void interrupt(void)
       //interruptStartTime = micros();
 
       interruptBusy = true;
-      StepperControl::getInstance()->handleMovementInterrupt();
+      Movement::getInstance()->handleMovementInterrupt();
       interruptBusy = false;
     }
   }
 }
 #endif
 
+/**/ // unsigned long intrCounter = 0;
+
 #if defined(FARMDUINO_EXP_V20)
 ISR(TIMER2_OVF_vect) {
 
   if (interruptBusy == false)
   {
     interruptBusy = true;
-    StepperControl::getInstance()->handleMovementInterrupt();
+
+    Movement::getInstance()->handleMovementInterrupt();
     interruptBusy = false;
   }
 }
 #endif
 
-//The setup function is called once at startup of the sketch
-void setup()
+void checkPinGuard()
 {
-  #ifdef RAMPS_V14
-
-    // Setup pin input/output settings
-    pinMode(X_STEP_PIN, OUTPUT);
-    pinMode(X_DIR_PIN, OUTPUT);
-    pinMode(X_ENABLE_PIN, OUTPUT);
-    pinMode(E_STEP_PIN, OUTPUT);
-    pinMode(E_DIR_PIN, OUTPUT);
-    pinMode(E_ENABLE_PIN, OUTPUT);
-    pinMode(X_MIN_PIN, INPUT_PULLUP);
-    pinMode(X_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(X_ENCDR_A, INPUT_PULLUP);
-    pinMode(X_ENCDR_B, INPUT_PULLUP);
-    pinMode(X_ENCDR_A_Q, INPUT_PULLUP);
-    pinMode(X_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(Y_STEP_PIN, OUTPUT);
-    pinMode(Y_DIR_PIN, OUTPUT);
-    pinMode(Y_ENABLE_PIN, OUTPUT);
-    pinMode(Y_MIN_PIN, INPUT_PULLUP);
-    pinMode(Y_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(Y_ENCDR_A, INPUT_PULLUP);
-    pinMode(Y_ENCDR_B, INPUT_PULLUP);
-    pinMode(Y_ENCDR_A_Q, INPUT_PULLUP);
-    pinMode(Y_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(Z_STEP_PIN, OUTPUT);
-    pinMode(Z_DIR_PIN, OUTPUT);
-    pinMode(Z_ENABLE_PIN, OUTPUT);
-    pinMode(Z_MIN_PIN, INPUT_PULLUP);
-    pinMode(Z_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(Z_ENCDR_A, INPUT_PULLUP);
-    pinMode(Z_ENCDR_B, INPUT_PULLUP);
-    pinMode(Z_ENCDR_A_Q, INPUT_PULLUP);
-    pinMode(Z_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(HEATER_0_PIN, OUTPUT);
-    pinMode(HEATER_1_PIN, OUTPUT);
-    pinMode(FAN_PIN, OUTPUT);
-    pinMode(LED_PIN, OUTPUT);
-
-    pinMode(UTM_C, INPUT_PULLUP);
-    pinMode(UTM_D, INPUT_PULLUP);
-    pinMode(UTM_E, INPUT_PULLUP);
-    pinMode(UTM_F, INPUT_PULLUP);
-    pinMode(UTM_G, INPUT_PULLUP);
-    pinMode(UTM_H, INPUT_PULLUP);
-    pinMode(UTM_I, INPUT_PULLUP);
-    pinMode(UTM_J, INPUT_PULLUP);
-    pinMode(UTM_K, INPUT_PULLUP);
-    pinMode(UTM_L, INPUT_PULLUP);
-
-    // Aux 1 pins to safer state
-    pinMode(AUX1_00, INPUT_PULLUP);
-    pinMode(AUX1_01, INPUT_PULLUP);
-    pinMode(AUX1_57, INPUT_PULLUP);
-    pinMode(AUX1_58, INPUT_PULLUP);
-
-    // Aux 3 pins to safer state
-    pinMode(AUX3_49, INPUT_PULLUP);
-    pinMode(AUX3_50, INPUT_PULLUP);
-    pinMode(AUX3_51, INPUT_PULLUP);
-
-    // Aux 4 pins to safer state
-    pinMode(AUX4_43, INPUT_PULLUP);
-    pinMode(AUX4_45, INPUT_PULLUP);
-    pinMode(AUX4_47, INPUT_PULLUP);
-    pinMode(AUX4_32, INPUT_PULLUP);
-
-    pinMode(SERVO_0_PIN, OUTPUT);
-    pinMode(SERVO_1_PIN, OUTPUT);
-    pinMode(SERVO_2_PIN, OUTPUT);
-    pinMode(SERVO_3_PIN, OUTPUT);
-
-  #endif
-
-  #if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
-
-    // Setup pin input/output settings
-    pinMode(X_STEP_PIN, OUTPUT);
-    pinMode(X_DIR_PIN, OUTPUT);
-    pinMode(X_ENABLE_PIN, OUTPUT);
-
-    pinMode(E_STEP_PIN, OUTPUT);
-    pinMode(E_DIR_PIN, OUTPUT);
-    pinMode(E_ENABLE_PIN, OUTPUT);
-    pinMode(X_MIN_PIN, INPUT_PULLUP);
-    pinMode(X_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(X_ENCDR_A, INPUT_PULLUP);
-    pinMode(X_ENCDR_B, INPUT_PULLUP);
-    //pinMode(X_ENCDR_A_Q, INPUT_PULLUP);
-    //pinMode(X_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(Y_STEP_PIN, OUTPUT);
-    pinMode(Y_DIR_PIN, OUTPUT);
-    pinMode(Y_ENABLE_PIN, OUTPUT);
-    pinMode(Y_MIN_PIN, INPUT_PULLUP);
-    pinMode(Y_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(Y_ENCDR_A, INPUT_PULLUP);
-    pinMode(Y_ENCDR_B, INPUT_PULLUP);
-    //pinMode(Y_ENCDR_A_Q, INPUT_PULLUP);
-    //pinMode(Y_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(Z_STEP_PIN, OUTPUT);
-    pinMode(Z_DIR_PIN, OUTPUT);
-    pinMode(Z_ENABLE_PIN, OUTPUT);
-    pinMode(Z_MIN_PIN, INPUT_PULLUP);
-    pinMode(Z_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(Z_ENCDR_A, INPUT_PULLUP);
-    pinMode(Z_ENCDR_B, INPUT_PULLUP);
-    //pinMode(Z_ENCDR_A_Q, INPUT_PULLUP);
-    //pinMode(Z_ENCDR_B_Q, INPUT_PULLUP);
-
-    pinMode(AUX_STEP_PIN, OUTPUT);
-    pinMode(AUX_DIR_PIN, OUTPUT);
-    pinMode(AUX_ENABLE_PIN, OUTPUT);
-    digitalWrite(AUX_ENABLE_PIN, HIGH);
-
-    pinMode(LED_PIN, OUTPUT);
-    pinMode(VACUUM_PIN, OUTPUT);
-    pinMode(WATER_PIN, OUTPUT);
-    pinMode(LIGHTING_PIN, OUTPUT);
-    pinMode(PERIPHERAL_4_PIN, OUTPUT);
-    pinMode(PERIPHERAL_5_PIN, OUTPUT);
-
-    pinMode(UTM_C, INPUT_PULLUP);
-    pinMode(UTM_D, INPUT_PULLUP);
-    if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); };
-    if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); };
-    if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); };
-    if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); };
-    if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); };
-    if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); };
-    if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); };
-    if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); };
-
-    pinMode(SERVO_0_PIN, OUTPUT);
-    pinMode(SERVO_1_PIN, OUTPUT);
-    pinMode(SERVO_2_PIN, OUTPUT);
-    pinMode(SERVO_3_PIN, OUTPUT);
-
-  #endif
-
-  #if defined(FARMDUINO_V14)
-
-    reportingPeriod = 500;
-
-    pinMode(READ_ENA_PIN, INPUT_PULLUP);
-    pinMode(NSS_PIN, OUTPUT);
-    digitalWrite(NSS_PIN, HIGH);
-
-    SPI.setBitOrder(MSBFIRST);
-    SPI.setDataMode(SPI_MODE0);
-    SPI.setClockDivider(SPI_CLOCK_DIV4);
-    SPI.begin();
-
-  #endif
-
-  #if defined(FARMDUINO_EXP_V20)
-
-    // Motor step, directio and pin is setup using the control chip library
-    // This board also does not use encoders
-
-    pinMode(X_ENABLE_PIN, OUTPUT);
-    pinMode(X_MIN_PIN, INPUT_PULLUP);
-    pinMode(X_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(E_ENABLE_PIN, OUTPUT);
-
-    pinMode(Y_ENABLE_PIN, OUTPUT);
-    pinMode(Y_MIN_PIN, INPUT_PULLUP);
-    pinMode(Y_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(Z_ENABLE_PIN, OUTPUT);
-    pinMode(Z_MIN_PIN, INPUT_PULLUP);
-    pinMode(Z_MAX_PIN, INPUT_PULLUP);
-
-    pinMode(AUX_STEP_PIN, OUTPUT);
-    pinMode(AUX_DIR_PIN, OUTPUT);
-    pinMode(AUX_ENABLE_PIN, OUTPUT);
-
-    pinMode(LED_PIN, OUTPUT);
-    pinMode(VACUUM_PIN, OUTPUT);
-    pinMode(WATER_PIN, OUTPUT);
-    pinMode(LIGHTING_PIN, OUTPUT);
-    pinMode(PERIPHERAL_4_PIN, OUTPUT);
-    pinMode(PERIPHERAL_5_PIN, OUTPUT);
-
-    pinMode(UTM_C, INPUT_PULLUP);
-    pinMode(UTM_D, INPUT_PULLUP);
-    if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); };
-    if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); };
-    if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); };
-    if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); };
-    if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); };
-    if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); };
-    if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); };
-    if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); };
-
-    pinMode(SERVO_0_PIN, OUTPUT);
-    pinMode(SERVO_1_PIN, OUTPUT);
-    pinMode(SERVO_2_PIN, OUTPUT);
-    pinMode(SERVO_3_PIN, OUTPUT);
-
-  #endif
-
-  digitalWrite(X_ENABLE_PIN, HIGH);
-  digitalWrite(E_ENABLE_PIN, HIGH);
-  digitalWrite(Y_ENABLE_PIN, HIGH);
-  digitalWrite(Z_ENABLE_PIN, HIGH);
-
-  Serial.begin(115200);
-  delay(100);
-
-  // Start the motor handling
-  //ServoControl::getInstance()->attach();
-
-  // Load motor settings
-  StepperControl::getInstance()->loadSettings();
-
-  // Dump all values to the serial interface
-  // ParameterList::getInstance()->readAllValues();
-
-  // Get the settings for the pin guard
-  PinGuard::getInstance()->loadConfig();
-  pinGuardLastCheck = millis();
-
-  // Start the interrupt used for moving
-  // Interrupt management code library written by Paul Stoffregen
-  // The default time 100 micro seconds
-
-  #if !defined(FARMDUINO_EXP_V20)
-    Timer1.attachInterrupt(interrupt);
-    Timer1.initialize(MOVEMENT_INTERRUPT_SPEED);
-    Timer1.start();
-  #endif
-
-  #if defined(FARMDUINO_EXP_V20)
-    TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow
-    TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1
-    OCR2A = 4; // Set overflow to 4 for total of 64 µs    
-  #endif
-
-  // Initialize the inactivity check
-  lastAction = millis();
-
-  pinGuardCurrentTime = millis();
-  pinGuardLastCheck = millis();
-
-  if
-  (
-    (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
-    (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1)
-  )
-  {
-    Serial.print("R99 HOME Z ON STARTUP\r\n");
-    StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
-  }
-
-  if
-  (
-    (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
-    (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Y) == 1)
-  )
-  {
-    Serial.print("R99 HOME Y ON STARTUP\r\n");
-    StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false);
-  }
-
-  if
-  (
-    (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
-    (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_X) == 1)
-  )
-  {
-    Serial.print("R99 HOME X ON STARTUP\r\n");
-    StepperControl::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false);
-  }
-
-
-  #if defined(FARMDUINO_EXP_V20)
-    // initialise the motors
-    StepperControl::getInstance()->initTMC2130();
-    StepperControl::getInstance()->loadSettingsTMC2130();
-  #endif
-
-  Serial.print("R99 ARDUINO STARTUP COMPLETE\r\n");
-
-  //StepperControl::getInstance()->test2();
-}
-
-char commandChar[INCOMING_CMD_BUF_SIZE + 1];
-
-/**/ //int cycleCountTest = 0;
-
-// The loop function is called in an endless loop
-void loop()
-{
-
-  //Serial.print(millis());
-  //Serial.print("\r\n");
-  //delay(1000);
-
-  //StepperControl::getInstance()->test();
-  //delayMicroseconds(100);
-
-  //digitalWrite(LED_PIN, true);
-  //delay(250);
-  //digitalWrite(LED_PIN, false);
-  //delay(250);
-
-  if (debugInterrupt)
-  {
-    StepperControl::getInstance()->handleMovementInterrupt();
-  }
-
-  #if defined(FARMDUINO_V14)
-    // Check encoders out of interrupt for farmduino 1.4
-    StepperControl::getInstance()->checkEncoders();
-  #endif
-
   pinGuardCurrentTime = millis();
   if (pinGuardCurrentTime < pinGuardLastCheck)
   {
@@ -434,7 +102,84 @@ void loop()
       PinGuard::getInstance()->checkPins();
     }
   }
+}
 
+void periodicChecksAndReport()
+{
+
+  // Do periodic checks and feedback
+  currentTime = millis();
+  if (currentTime < lastAction)
+  {
+
+    // If the device timer overruns, reset the idle counter
+    lastAction = millis();
+  }
+  else
+  {
+
+    if ((currentTime - lastAction) > reportingPeriod)
+    {
+      // After an idle time, send the idle message
+
+      if (CurrentState::getInstance()->isEmergencyStop())
+      {
+        Serial.print(COMM_REPORT_EMERGENCY_STOP);
+        CurrentState::getInstance()->printQAndNewLine();
+
+        if (debugMessages)
+        {
+          Serial.print(COMM_REPORT_COMMENT);
+          Serial.print(" Emergency stop engaged");
+          CurrentState::getInstance()->printQAndNewLine();
+        }
+      }
+      else
+      {
+        Serial.print(COMM_REPORT_CMD_IDLE);
+        CurrentState::getInstance()->printQAndNewLine();
+      }
+
+      Movement::getInstance()->storePosition();
+      CurrentState::getInstance()->printPosition();
+
+      Movement::getInstance()->reportEncoders();
+      CurrentState::getInstance()->storeEndStops();
+      CurrentState::getInstance()->printEndStops();
+
+      // Movement::getInstance()->test();
+
+      if (debugMessages)
+      {
+        Serial.print(COMM_REPORT_COMMENT);
+        Serial.print(" MEM ");
+        Serial.print(freeMemory());
+        CurrentState::getInstance()->printQAndNewLine();
+
+        Serial.print(COMM_REPORT_COMMENT);
+        Serial.print(" IND DUR ");
+        Serial.print(interruptDuration);
+        Serial.print(" MAX ");
+        Serial.print(interruptDurationMax);
+        CurrentState::getInstance()->printQAndNewLine();
+
+        //Movement::getInstance()->test();
+      }
+
+      if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1)
+      {
+        Serial.print(COMM_REPORT_NO_CONFIG);
+        CurrentState::getInstance()->printQAndNewLine();
+      }
+
+      cycleCounter++;
+      lastAction = millis();
+    }
+  }
+}
+
+void checkSerialInputs()
+{
   if (Serial.available())
   {
     // Save current time stamp for timeout actions
@@ -501,12 +246,15 @@ void loop()
       incomingCommandPointer = 0;
     }
   }
+}
 
+void checkEmergencyStop()
+{
   // In case of emergency stop, disable movement and
   // shut down the pins used
   if (previousEmergencyStop == false && CurrentState::getInstance()->isEmergencyStop())
   {
-    StepperControl::getInstance()->disableMotorsEmergency();
+    Movement::getInstance()->disableMotorsEmergency();
     PinControl::getInstance()->resetPinsUsed();
     ServoControl::getInstance()->detachServos();
     if (debugMessages)
@@ -517,7 +265,10 @@ void loop()
     }
   }
   previousEmergencyStop = CurrentState::getInstance()->isEmergencyStop();
+}
 
+void checkParamsChanged()
+{
   // Check if parameters are changed, and if so load the new settings
   if (lastParamChangeNr != ParameterList::getInstance()->paramChangeNumber())
   {
@@ -530,85 +281,633 @@ void loop()
       CurrentState::getInstance()->printQAndNewLine();
     }
 
-    StepperControl::getInstance()->loadSettings();
+    /*
+    #if defined(FARMDUINO_EXP_V20)
+    loadTMC2130paraeters();
+    #endif
+    */
+
+    Movement::getInstance()->loadSettings();
     PinGuard::getInstance()->loadConfig();
   }
+}
 
-  // Do periodic checks and feedback
+void checkEncoders()
+{
+  
+  #if defined(FARMDUINO_V14)
+    // Check encoders out of interrupt for farmduino 1.4
+    Movement::getInstance()->checkEncoders();
+  #endif
+  
+}
 
-  currentTime = millis();
-  if (currentTime < lastAction)
-  {
+void runTestForDebug()
+{
 
-    // If the device timer overruns, reset the idle counter
-    lastAction = millis();
-  }
-  else
-  {
+  //Serial.print("* ");
+  //Serial.print(intrCounter / 1000);
+  //Serial.print("\r\n");
+  //delay(500);
 
-    if ((currentTime - lastAction) > reportingPeriod)
-    {
-      // After an idle time, send the idle message
+  //Serial.print("z");
+  //blinkLed();
+  //delay(500);
 
-      if (CurrentState::getInstance()->isEmergencyStop())
-      {
-        Serial.print(COMM_REPORT_EMERGENCY_STOP);
-        CurrentState::getInstance()->printQAndNewLine();
+  //Serial.print(millis());
+  //Serial.print("X");
+  //Serial.print("\r\n");
+  //delay(1000);
 
-        if (debugMessages)
-        {
-          Serial.print(COMM_REPORT_COMMENT);
-          Serial.print(" Emergency stop engaged");
-          CurrentState::getInstance()->printQAndNewLine();
-        }
-      }
-      else
-      {
-        Serial.print(COMM_REPORT_CMD_IDLE);
-        CurrentState::getInstance()->printQAndNewLine();
-      }
+  //blinkLed();
 
-      StepperControl::getInstance()->storePosition();
-      CurrentState::getInstance()->printPosition();
+  //Movement::getInstance()->test();
+  //delayMicroseconds(100);
 
-      StepperControl::getInstance()->reportEncoders();
+  //digitalWrite(LED_PIN, true);
+  //delay(250);
+  //digitalWrite(LED_PIN, false);
+  //delay(250);
 
-      CurrentState::getInstance()->storeEndStops();
-      CurrentState::getInstance()->printEndStops();
+  //if (debugInterrupt)
+  //{
+  //  Movement::getInstance()->handleMovementInterrupt();
+  //}
+}
 
-      /**/ //cycleCountTest++;
-      /**/ //Serial.print("R99 TST Cycle count ");
-      /**/ //Serial.print(cycleCountTest);
-      /**/ //Serial.print(" ");
-      /**/ //CurrentState::getInstance()->printQAndNewLine();
+// Set pins input output
+#ifdef RAMPS_V14
+void setPinInputOutput()
+{
 
+  // pin input/output settings
+  pinMode(X_STEP_PIN, OUTPUT);
+  pinMode(X_DIR_PIN, OUTPUT);
+  pinMode(X_ENABLE_PIN, OUTPUT);
+  pinMode(E_STEP_PIN, OUTPUT);
+  pinMode(E_DIR_PIN, OUTPUT);
+  pinMode(E_ENABLE_PIN, OUTPUT);
+  pinMode(X_MIN_PIN, INPUT_PULLUP);
+  pinMode(X_MAX_PIN, INPUT_PULLUP);
 
-      if (debugMessages)
-      {
-        Serial.print(COMM_REPORT_COMMENT);
-        Serial.print(" MEM ");
-        Serial.print(freeMemory());
-        CurrentState::getInstance()->printQAndNewLine();
+  pinMode(X_ENCDR_A, INPUT_PULLUP);
+  pinMode(X_ENCDR_B, INPUT_PULLUP);
+  pinMode(X_ENCDR_A_Q, INPUT_PULLUP);
+  pinMode(X_ENCDR_B_Q, INPUT_PULLUP);
 
-        Serial.print(COMM_REPORT_COMMENT);
-        Serial.print(" IND DUR ");
-        Serial.print(interruptDuration);
-        Serial.print(" MAX ");
-        Serial.print(interruptDurationMax);
-        CurrentState::getInstance()->printQAndNewLine();
+  pinMode(Y_STEP_PIN, OUTPUT);
+  pinMode(Y_DIR_PIN, OUTPUT);
+  pinMode(Y_ENABLE_PIN, OUTPUT);
+  pinMode(Y_MIN_PIN, INPUT_PULLUP);
+  pinMode(Y_MAX_PIN, INPUT_PULLUP);
 
-        StepperControl::getInstance()->test();
-      }
+  pinMode(Y_ENCDR_A, INPUT_PULLUP);
+  pinMode(Y_ENCDR_B, INPUT_PULLUP);
+  pinMode(Y_ENCDR_A_Q, INPUT_PULLUP);
+  pinMode(Y_ENCDR_B_Q, INPUT_PULLUP);
 
-      if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1)
-      {
-        Serial.print(COMM_REPORT_NO_CONFIG);
-        CurrentState::getInstance()->printQAndNewLine();
-      }
+  pinMode(Z_STEP_PIN, OUTPUT);
+  pinMode(Z_DIR_PIN, OUTPUT);
+  pinMode(Z_ENABLE_PIN, OUTPUT);
+  pinMode(Z_MIN_PIN, INPUT_PULLUP);
+  pinMode(Z_MAX_PIN, INPUT_PULLUP);
 
-      cycleCounter++;
-      lastAction = millis();
-    }
-  }
+  pinMode(Z_ENCDR_A, INPUT_PULLUP);
+  pinMode(Z_ENCDR_B, INPUT_PULLUP);
+  pinMode(Z_ENCDR_A_Q, INPUT_PULLUP);
+  pinMode(Z_ENCDR_B_Q, INPUT_PULLUP);
+
+  pinMode(HEATER_0_PIN, OUTPUT);
+  pinMode(HEATER_1_PIN, OUTPUT);
+  pinMode(FAN_PIN, OUTPUT);
+  pinMode(LED_PIN, OUTPUT);
+
+  pinMode(UTM_C, INPUT_PULLUP);
+  pinMode(UTM_D, INPUT_PULLUP);
+  pinMode(UTM_E, INPUT_PULLUP);
+  pinMode(UTM_F, INPUT_PULLUP);
+  pinMode(UTM_G, INPUT_PULLUP);
+  pinMode(UTM_H, INPUT_PULLUP);
+  pinMode(UTM_I, INPUT_PULLUP);
+  pinMode(UTM_J, INPUT_PULLUP);
+  pinMode(UTM_K, INPUT_PULLUP);
+  pinMode(UTM_L, INPUT_PULLUP);
+
+  // Aux 1 pins to safer state
+  pinMode(AUX1_00, INPUT_PULLUP);
+  pinMode(AUX1_01, INPUT_PULLUP);
+  pinMode(AUX1_57, INPUT_PULLUP);
+  pinMode(AUX1_58, INPUT_PULLUP);
+
+  // Aux 3 pins to safer state
+  pinMode(AUX3_49, INPUT_PULLUP);
+  pinMode(AUX3_50, INPUT_PULLUP);
+  pinMode(AUX3_51, INPUT_PULLUP);
+
+  // Aux 4 pins to safer state
+  pinMode(AUX4_43, INPUT_PULLUP);
+  pinMode(AUX4_45, INPUT_PULLUP);
+  pinMode(AUX4_47, INPUT_PULLUP);
+  pinMode(AUX4_32, INPUT_PULLUP);
+
+  pinMode(SERVO_0_PIN, OUTPUT);
+  pinMode(SERVO_1_PIN, OUTPUT);
+  pinMode(SERVO_2_PIN, OUTPUT);
+  pinMode(SERVO_3_PIN, OUTPUT);
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Set input/output");
+  Serial.print(CRLF);
+}
+#endif
+
+#if defined(FARMDUINO_V10) || defined(FARMDUINO_V14)
+void setPinInputOutput()
+{
+
+  // pin input/output settings
+  pinMode(X_STEP_PIN, OUTPUT);
+  pinMode(X_DIR_PIN, OUTPUT);
+  pinMode(X_ENABLE_PIN, OUTPUT);
+
+  pinMode(E_STEP_PIN, OUTPUT);
+  pinMode(E_DIR_PIN, OUTPUT);
+  pinMode(E_ENABLE_PIN, OUTPUT);
+  pinMode(X_MIN_PIN, INPUT_PULLUP);
+  pinMode(X_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(X_ENCDR_A, INPUT_PULLUP);
+  pinMode(X_ENCDR_B, INPUT_PULLUP);
+  //pinMode(X_ENCDR_A_Q, INPUT_PULLUP);
+  //pinMode(X_ENCDR_B_Q, INPUT_PULLUP);
+
+  pinMode(Y_STEP_PIN, OUTPUT);
+  pinMode(Y_DIR_PIN, OUTPUT);
+  pinMode(Y_ENABLE_PIN, OUTPUT);
+  pinMode(Y_MIN_PIN, INPUT_PULLUP);
+  pinMode(Y_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(Y_ENCDR_A, INPUT_PULLUP);
+  pinMode(Y_ENCDR_B, INPUT_PULLUP);
+  //pinMode(Y_ENCDR_A_Q, INPUT_PULLUP);
+  //pinMode(Y_ENCDR_B_Q, INPUT_PULLUP);
+
+  pinMode(Z_STEP_PIN, OUTPUT);
+  pinMode(Z_DIR_PIN, OUTPUT);
+  pinMode(Z_ENABLE_PIN, OUTPUT);
+  pinMode(Z_MIN_PIN, INPUT_PULLUP);
+  pinMode(Z_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(Z_ENCDR_A, INPUT_PULLUP);
+  pinMode(Z_ENCDR_B, INPUT_PULLUP);
+  //pinMode(Z_ENCDR_A_Q, INPUT_PULLUP);
+  //pinMode(Z_ENCDR_B_Q, INPUT_PULLUP);
+
+  pinMode(AUX_STEP_PIN, OUTPUT);
+  pinMode(AUX_DIR_PIN, OUTPUT);
+  pinMode(AUX_ENABLE_PIN, OUTPUT);
+  digitalWrite(AUX_ENABLE_PIN, HIGH);
+
+  pinMode(LED_PIN, OUTPUT);
+  pinMode(VACUUM_PIN, OUTPUT);
+  pinMode(WATER_PIN, OUTPUT);
+  pinMode(LIGHTING_PIN, OUTPUT);
+  pinMode(PERIPHERAL_4_PIN, OUTPUT);
+  pinMode(PERIPHERAL_5_PIN, OUTPUT);
+
+  pinMode(UTM_C, INPUT_PULLUP);
+  pinMode(UTM_D, INPUT_PULLUP);
+  if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); };
+  if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); };
+  if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); };
+  if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); };
+  if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); };
+  if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); };
+  if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); };
+  if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); };
+
+  pinMode(SERVO_0_PIN, OUTPUT);
+  pinMode(SERVO_1_PIN, OUTPUT);
+  pinMode(SERVO_2_PIN, OUTPUT);
+  pinMode(SERVO_3_PIN, OUTPUT);
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Set input/output");
+  Serial.print(CRLF);
+}
+#endif
+
+#if defined(FARMDUINO_V14)
+void setPinInputOutput()
+{
+
+  reportingPeriod = 500;
+
+  pinMode(READ_ENA_PIN, INPUT_PULLUP);
+  pinMode(NSS_PIN, OUTPUT);
+  digitalWrite(NSS_PIN, HIGH);
+
+  SPI.setBitOrder(MSBFIRST);
+  SPI.setDataMode(SPI_MODE0);
+  SPI.setClockDivider(SPI_CLOCK_DIV4);
+  SPI.begin();
 
 }
+#endif
+
+#if defined(FARMDUINO_EXP_V20)
+void setPinInputOutput()
+{
+
+  // Motor step, direction and pin is set up using the control chip library
+  // This board also does not use encoders
+
+  pinMode(X_ENABLE_PIN, OUTPUT);
+  pinMode(X_MIN_PIN, INPUT_PULLUP);
+  pinMode(X_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(E_ENABLE_PIN, OUTPUT);
+
+  pinMode(Y_ENABLE_PIN, OUTPUT);
+  pinMode(Y_MIN_PIN, INPUT_PULLUP);
+  pinMode(Y_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(Z_ENABLE_PIN, OUTPUT);
+  pinMode(Z_MIN_PIN, INPUT_PULLUP);
+  pinMode(Z_MAX_PIN, INPUT_PULLUP);
+
+  pinMode(AUX_STEP_PIN, OUTPUT);
+  pinMode(AUX_DIR_PIN, OUTPUT);
+  pinMode(AUX_ENABLE_PIN, OUTPUT);
+
+  pinMode(LED_PIN, OUTPUT);
+  pinMode(VACUUM_PIN, OUTPUT);
+  pinMode(WATER_PIN, OUTPUT);
+  pinMode(LIGHTING_PIN, OUTPUT);
+  pinMode(PERIPHERAL_4_PIN, OUTPUT);
+  pinMode(PERIPHERAL_5_PIN, OUTPUT);
+
+  pinMode(UTM_C, INPUT_PULLUP);
+  pinMode(UTM_D, INPUT_PULLUP);
+  if (UTM_E > 0) { pinMode(UTM_E, INPUT_PULLUP); };
+  if (UTM_F > 0) { pinMode(UTM_F, INPUT_PULLUP); };
+  if (UTM_G > 0) { pinMode(UTM_G, INPUT_PULLUP); };
+  if (UTM_H > 0) { pinMode(UTM_H, INPUT_PULLUP); };
+  if (UTM_I > 0) { pinMode(UTM_I, INPUT_PULLUP); };
+  if (UTM_J > 0) { pinMode(UTM_J, INPUT_PULLUP); };
+  if (UTM_K > 0) { pinMode(UTM_K, INPUT_PULLUP); };
+  if (UTM_L > 0) { pinMode(UTM_L, INPUT_PULLUP); };
+
+  pinMode(SERVO_0_PIN, OUTPUT);
+  pinMode(SERVO_1_PIN, OUTPUT);
+  pinMode(SERVO_2_PIN, OUTPUT);
+  pinMode(SERVO_3_PIN, OUTPUT);
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Set input/output");
+  Serial.print(CRLF);
+}
+#endif
+
+// other initialisation functions
+void startSerial()
+{
+  Serial.begin(115200);
+  delay(100);
+  while (!Serial);
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Serial connection started");
+  Serial.print(CRLF);
+}
+
+
+#if defined(FARMDUINO_EXP_V20)
+void startupTmc()
+{
+
+  // Initialize the drivers
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Init TMC2130 drivers");
+  Serial.print(CRLF);
+
+  TMC2130X->begin();
+  TMC2130Y->begin();
+  TMC2130Z->begin();
+  TMC2130E->begin();
+
+  // Load the motor parameters
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Load TMC2130 parameters");
+  Serial.print(CRLF);
+
+  Movement::getInstance()->initTMC2130();
+  Movement::getInstance()->loadSettingsTMC2130();
+
+
+  /*
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Set shaft dir");
+  Serial.print(CRLF);
+
+  TMC2130X->shaft_dir(0);
+  TMC2130Y->shaft_dir(0);
+  TMC2130Z->shaft_dir(0);
+  TMC2130E->shaft_dir(0);
+  */
+}
+#endif
+
+void startInterrupt()
+{
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Start interrupt");
+  Serial.print(CRLF);
+
+
+  // Start the interrupt used for moving
+  // Interrupt management code library written by Paul Stoffregen
+  // The default time 100 micro seconds
+
+  #if !defined(FARMDUINO_EXP_V20)
+    Timer1.attachInterrupt(interrupt);
+    Timer1.initialize(MOVEMENT_INTERRUPT_SPEED);
+    Timer1.start();
+  #endif
+
+  #if defined(FARMDUINO_EXP_V20)
+    Serial.println("set timer");
+        TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow
+        TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1
+        OCR2A = 4; // Set overflow to 4 for total of 64 µs
+  #endif
+}
+
+void homeOnBoot()
+{
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Check homing on boot");
+  Serial.print(CRLF);
+
+  if
+  (
+  (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
+  (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Z) == 1)
+  )
+  {
+    Serial.print("R99 HOME Z ON STARTUP\r\n");
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
+  }
+
+  if
+  (
+  (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
+  (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_Y) == 1)
+  )
+  {
+    Serial.print("R99 HOME Y ON STARTUP\r\n");
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, true, false);
+  }
+
+  if
+  (
+  (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) == 1) &&
+  (ParameterList::getInstance()->getValue(MOVEMENT_HOME_AT_BOOT_X) == 1)
+  )
+  {
+    Serial.print("R99 HOME X ON STARTUP\r\n");
+    Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, true, false, false);
+  }
+}
+
+void startMotor()
+{
+  // Start the motor handling
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Set motor enables off");
+  Serial.print(CRLF);
+
+  digitalWrite(X_ENABLE_PIN, HIGH);
+  digitalWrite(E_ENABLE_PIN, HIGH);
+  digitalWrite(Y_ENABLE_PIN, HIGH);
+  digitalWrite(Z_ENABLE_PIN, HIGH);
+}
+
+void loadMovementSetting()
+{
+
+  // Load motor settings
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Load movement settings");
+  Serial.print(CRLF);
+
+  Movement::getInstance()->loadSettings();
+}
+
+void readParameters()
+{
+  // Dump all values to the serial interface
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Read parameters");
+  Serial.print(CRLF);
+
+  ParameterList::getInstance()->readAllValues();
+}
+
+void startPinGuard()
+{
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Start pin guard");
+  Serial.print(CRLF);
+
+  // Get the settings for the pin guard
+  PinGuard::getInstance()->loadConfig();
+
+  pinGuardCurrentTime = millis();
+  pinGuardLastCheck = millis();
+}
+
+void startServo()
+{
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Start servo");
+  Serial.print(CRLF);
+
+  ServoControl::getInstance()->attach();
+}
+
+#if defined(FARMDUINO_EXP_V20)
+void loadTMC2130drivers()
+{
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("Load TMC drivers");
+  Serial.print(CRLF);
+
+  TMC2130Stepper controllerTMC2130_X = TMC2130Stepper(X_CHIP_SELECT);
+  TMC2130Stepper controllerTMC2130_Y = TMC2130Stepper(Y_CHIP_SELECT);
+  TMC2130Stepper controllerTMC2130_Z = TMC2130Stepper(Z_CHIP_SELECT);
+  TMC2130Stepper controllerTMC2130_E = TMC2130Stepper(E_CHIP_SELECT);
+
+  TMC2130X = &controllerTMC2130_X;
+  TMC2130Y = &controllerTMC2130_Y;
+  TMC2130Z = &controllerTMC2130_Z;
+  TMC2130E = &controllerTMC2130_E;
+
+  int motorCurrentX;
+  int stallSensitivityX;
+  int microStepsX;
+
+  int motorCurrentY;
+  int stallSensitivityY;
+  int microStepsY;
+
+  int motorCurrentZ;
+  int stallSensitivityZ;
+  int microStepsZ;
+
+  motorCurrentX = 600;
+  stallSensitivityX = 0;
+  microStepsX = 0;
+
+  motorCurrentY = 600;
+  stallSensitivityY = 0;
+  microStepsY = 0;
+
+  motorCurrentZ = 600;
+  stallSensitivityZ = 0;
+  microStepsZ = 0;
+
+  motorCurrentX = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_X);
+  stallSensitivityX = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_X);
+  microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_X);
+
+  motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y);
+  stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y);
+  microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y);
+
+  motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z);
+  stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z);
+  microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z);
+
+/*
+  TMC2130X->push();
+  TMC2130X->toff(3);
+  TMC2130X->tbl(1);
+  TMC2130X->hysteresis_start(4);
+  TMC2130X->hysteresis_end(-2);
+  TMC2130X->rms_current(motorCurrentX); // mA
+  TMC2130X->microsteps(microStepsX);
+  TMC2130X->diag1_stall(1);
+  TMC2130X->diag1_active_high(1);
+  TMC2130X->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130X->THIGH(0);
+  TMC2130X->semin(5);
+  TMC2130X->semax(2);
+  TMC2130X->sedn(0b01);
+  TMC2130X->sg_stall_value(stallSensitivityX);
+
+  TMC2130Y->push();
+  TMC2130Y->toff(3);
+  TMC2130Y->tbl(1);
+  TMC2130Y->hysteresis_start(4);
+  TMC2130Y->hysteresis_end(-2);
+  TMC2130Y->rms_current(motorCurrentY); // mA
+  TMC2130Y->microsteps(microStepsY);
+  TMC2130Y->diag1_stall(1);
+  TMC2130Y->diag1_active_high(1);
+  TMC2130Y->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130Y->THIGH(0);
+  TMC2130Y->semin(5);
+  TMC2130Y->semax(2);
+  TMC2130Y->sedn(0b01);
+  TMC2130Y->sg_stall_value(stallSensitivityY);
+
+  TMC2130Z->push();
+  TMC2130Z->toff(3);
+  TMC2130Z->tbl(1);
+  TMC2130Z->hysteresis_start(4);
+  TMC2130Z->hysteresis_end(-2);
+  TMC2130Z->rms_current(motorCurrentZ); // mA
+  TMC2130Z->microsteps(microStepsZ);
+  TMC2130Z->diag1_stall(1);
+  TMC2130Z->diag1_active_high(1);
+  TMC2130Z->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130Z->THIGH(0);
+  TMC2130Z->semin(5);
+  TMC2130Z->semax(2);
+  TMC2130Z->sedn(0b01);
+  TMC2130Z->sg_stall_value(stallSensitivityZ);
+
+  TMC2130E->push();
+  TMC2130E->toff(3);
+  TMC2130E->tbl(1);
+  TMC2130E->hysteresis_start(4);
+  TMC2130E->hysteresis_end(-2);
+  TMC2130E->rms_current(motorCurrentX); // mA
+  TMC2130E->microsteps(microStepsX);
+  TMC2130E->diag1_stall(1);
+  TMC2130E->diag1_active_high(1);
+  TMC2130E->coolstep_min_speed(0xFFFFF); // 20bit max
+  TMC2130E->THIGH(0);
+  TMC2130E->semin(5);
+  TMC2130E->semax(2);
+  TMC2130E->sedn(0b01);
+  TMC2130E->sg_stall_value(stallSensitivityX);
+*/
+
+
+  TMC2130X->rms_current(motorCurrentX);   // Set the required current in mA  
+  TMC2130X->microsteps(microStepsX);      // Minimum of micro steps needed
+  TMC2130X->chm(true);                    // Set the chopper mode to classic const. off time
+  TMC2130X->diag1_stall(1);               // Activate stall diagnostics
+  TMC2130X->sgt(stallSensitivityX);       // Set stall detection sensitivity. most -64 to +64 least
+  TMC2130X->shaft_dir(0);                 // Set direction
+
+  TMC2130Y->rms_current(motorCurrentX);   // Set the required current in mA  
+  TMC2130Y->microsteps(microStepsX);      // Minimum of micro steps needed
+  TMC2130Y->chm(true);                    // Set the chopper mode to classic const. off time
+  TMC2130Y->diag1_stall(1);               // Activate stall diagnostics
+  TMC2130Y->sgt(stallSensitivityX);       // Set stall detection sensitivity. most -64 to +64 least
+  TMC2130Y->shaft_dir(0);                 // Set direction
+
+  TMC2130Z->rms_current(motorCurrentX);   // Set the required current in mA  
+  TMC2130Z->microsteps(microStepsX);      // Minimum of micro steps needed
+  TMC2130Z->chm(true);                    // Set the chopper mode to classic const. off time
+  TMC2130Z->diag1_stall(1);               // Activate stall diagnostics
+  TMC2130Z->sgt(stallSensitivityX);       // Set stall detection sensitivity. most -64 to +64 least
+  TMC2130Z->shaft_dir(0);                 // Set direction
+
+  TMC2130E->rms_current(motorCurrentX);   // Set the required current in mA  
+  TMC2130E->microsteps(microStepsX);      // Minimum of micro steps needed
+  TMC2130E->chm(true);                    // Set the chopper mode to classic const. off time
+  TMC2130E->diag1_stall(1);               // Activate stall diagnostics
+  TMC2130E->sgt(stallSensitivityX);       // Set stall detection sensitivity. most -64 to +64 least
+  TMC2130E->shaft_dir(0);                 // Set direction
+}
+
+void loadTMC2130parameters()
+{
+}
+#endif
+
+void initLastAction()
+{
+  // Initialize the inactivity check
+  lastAction = millis();
+}
\ No newline at end of file
diff --git a/src/farmbot_arduino_controller.h b/src/farmbot_arduino_controller.h
index 426026b..6029496 100644
--- a/src/farmbot_arduino_controller.h
+++ b/src/farmbot_arduino_controller.h
@@ -7,10 +7,31 @@
 #ifndef _farmbot_arduino_controller_H_
 #define _farmbot_arduino_controller_H_
 #include "Arduino.h"
+//#include "Arduino.h"
+
 //add your includes for the project farmbot_arduino_controller here
+#include "Board.h";
+#include "pins.h"
+#include "Config.h"
+#include "MemoryFree.h"
+#include "Debug.h"
+
+#include "TMC2130.h"
+
+/**/
+#include "Movement.h"
+#include "ServoControl.h"
+#include "PinGuard.h"
+#include "CurrentState.h"
+#include <SPI.h>
 #include "Command.h"
 #include "GCodeProcessor.h"
 
+
+//#include <TMC2130Stepper.h>
+//#include <TMC2130Stepper_REGDEFS.h>
+
+/*
 //end of add your includes here
 #ifdef __cplusplus
 extern "C" {
@@ -20,10 +41,35 @@ void setup();
 #ifdef __cplusplus
 } // extern "C"
 #endif
+*/
 
 //add your function definitions for the project farmbot_arduino_controller here
 
+  void setPinInputOutput();
+  void startSerial();
+
+  #if defined(FARMDUINO_EXP_V20)
+  void loadTMC2130drivers();
+  void loadTMC2130parameters();
+  void startupTmc();
+  #endif
+
+  void startMotor();
+  void readParameters();
+  void loadMovementSetting();
+  void startPinGuard();
+  void startServo();
+  void startInterrupt();
+  void homeOnBoot();
+  void runTestForDebug();
+  void checkEncoders();
+  void checkPinGuard();
+  void checkSerialInputs();
+  void checkEmergencyStop();
+  void checkParamsChanged();
+  void periodicChecksAndReport();
+  void initLastAction();
 
 
 //Do not add code below this line
-#endif /* _farmbot_arduino_controller_H_ */
+#endif /* _farmbot_arduino_controller_H_ */
\ No newline at end of file
diff --git a/src/src.ino b/src/src.ino
index 6d044d8..d3a65b4 100644
--- a/src/src.ino
+++ b/src/src.ino
@@ -1 +1,49 @@
-// All code in farmbot_arduino_controller.cpp
+/*
+ Name:		src.ino
+ Created:	11/14/2019 9:51:10 PM
+ Author:	Tim Evers
+*/
+
+#include "farmbot_arduino_controller.h"
+
+// the setup function runs once when you press reset or power the board
+void setup()
+{
+  setPinInputOutput();
+  startSerial();
+  readParameters();
+
+#if defined(FARMDUINO_EXP_V20)
+  loadTMC2130drivers();
+  //loadTMC2130parameters();
+  startupTmc();
+#endif
+
+  loadMovementSetting();
+  startMotor();
+  startPinGuard();
+  startServo();
+  startInterrupt();
+  initLastAction();
+  homeOnBoot();
+
+  Serial.print(COMM_REPORT_COMMENT);
+  Serial.print(SPACE);
+  Serial.print("ARDUINO STARTUP COMPLETE");
+  Serial.print(CRLF);
+
+}
+
+// the loop function runs over and over again until power down or reset
+void loop()
+{
+  //runTestForDebug();
+
+  checkEncoders();
+  checkPinGuard();
+  checkSerialInputs();
+  checkEmergencyStop();
+  checkParamsChanged();
+  periodicChecksAndReport();
+
+}
diff --git a/src/src.vcxproj b/src/src.vcxproj
index 2a7bccf..8c1fa72 100644
--- a/src/src.vcxproj
+++ b/src/src.vcxproj
@@ -47,10 +47,10 @@
       <WarningLevel>Level3</WarningLevel>
       <Optimization>Disabled</Optimization>
       <SDLCheck>true</SDLCheck>
-      <AdditionalIncludeDirectories>$(ProjectDir)..\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries;$(ProjectDir)..\..\..\..\Users\Bro\Documents\Arduino\libraries;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr\;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
-      <ForcedIncludeFiles>$(ProjectDir)__vm\.src.vsarduino.h;%(ForcedIncludeFiles)</ForcedIncludeFiles>
+      <AdditionalIncludeDirectories>$(ProjectDir)..\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
+      <ForcedIncludeFiles>$(ProjectDir)__vm\.farmbot-arduino-controller.vsarduino.h;%(ForcedIncludeFiles)</ForcedIncludeFiles>
       <IgnoreStandardIncludePath>false</IgnoreStandardIncludePath>
-      <PreprocessorDefinitions>__AVR_ATmega2560__;_VMDEBUG=1;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
+      <PreprocessorDefinitions>__AVR_atmega2560__;__AVR_ATmega2560__;_VMDEBUG=1;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
     </ClCompile>
     <Link>
       <GenerateDebugInformation>true</GenerateDebugInformation>
@@ -115,16 +115,24 @@
     <ClCompile Include="GCodeHandler.cpp" />
     <ClCompile Include="GCodeProcessor.cpp" />
     <ClCompile Include="MemoryFree.cpp" />
+    <ClCompile Include="Movement.cpp" />
+    <ClCompile Include="MovementAxis.cpp" />
+    <ClCompile Include="MovementEncoder.cpp" />
     <ClCompile Include="ParameterList.cpp" />
     <ClCompile Include="PinControl.cpp" />
     <ClCompile Include="PinGuard.cpp" />
     <ClCompile Include="PinGuardPin.cpp" />
     <ClCompile Include="ServoControl.cpp" />
     <ClCompile Include="StatusList.cpp" />
-    <ClCompile Include="StepperControl.cpp" />
-    <ClCompile Include="StepperControlAxis.cpp" />
-    <ClCompile Include="StepperControlEncoder.cpp" />
     <ClCompile Include="TimerOne.cpp" />
+    <ClCompile Include="TMC2130.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_CHOPCONF.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_COOLCONF.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_DRV_STATUS.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_GCONF.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_IHOLD_IRUN.cpp" />
+    <ClCompile Include="TMC2130\TMC2130Stepper_PWMCONF.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Board.h" />
@@ -160,6 +168,9 @@
     <ClInclude Include="GCodeProcessor.h" />
     <ClInclude Include="known_16bit_timers.h" />
     <ClInclude Include="MemoryFree.h" />
+    <ClInclude Include="Movement.h" />
+    <ClInclude Include="MovementAxis.h" />
+    <ClInclude Include="MovementEncoder.h" />
     <ClInclude Include="ParameterList.h" />
     <ClInclude Include="PinControl.h" />
     <ClInclude Include="PinGuard.h" />
@@ -167,10 +178,12 @@
     <ClInclude Include="pins.h" />
     <ClInclude Include="ServoControl.h" />
     <ClInclude Include="StatusList.h" />
-    <ClInclude Include="StepperControl.h" />
-    <ClInclude Include="StepperControlAxis.h" />
-    <ClInclude Include="StepperControlEncoder.h" />
     <ClInclude Include="TimerOne.h" />
+    <ClInclude Include="TMC2130.h" />
+    <ClInclude Include="TMC2130\TMC2130Stepper.h" />
+    <ClInclude Include="TMC2130\TMC2130Stepper_MACROS.h" />
+    <ClInclude Include="TMC2130\TMC2130Stepper_REGDEFS.h" />
+    <ClInclude Include="TMC2130\TMC2130Stepper_UTILITY.h" />
     <ClInclude Include="__vm\.farmbot-arduino-controller.vsarduino.h" />
     <ClInclude Include="__vm\.src.vsarduino.h" />
   </ItemGroup>
@@ -179,7 +192,7 @@
   </ImportGroup>
   <ProjectExtensions>
     <VisualStudio>
-      <UserProperties custom_mega_atmega2560_vm.programmer_name="" vm.programmer_name="avrisp" custom_mega_atmega2560_vm.upload.useprogrammer="0" arduino.upload.port="COM3" />
+      <UserProperties custom_mega_atmega2560_vm.programmer_name="" config.Release.customdebug_mega_atmega2560_debugger_type="none" vm.programmer_name="avrisp" config.Debug.customdebug_mega_atmega2560_debugger_type="universal" custom_mega_atmega2560_vm.upload.useprogrammer="0" arduino.upload.port="COM3" />
     </VisualStudio>
   </ProjectExtensions>
 </Project>
\ No newline at end of file
diff --git a/src/src.vcxproj.filters b/src/src.vcxproj.filters
index f6080e4..f4323fa 100644
--- a/src/src.vcxproj.filters
+++ b/src/src.vcxproj.filters
@@ -117,22 +117,46 @@
     <ClCompile Include="ServoControl.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="StepperControlAxis.cpp">
-      <Filter>Source Files</Filter>
-    </ClCompile>
-    <ClCompile Include="StepperControlEncoder.cpp">
-      <Filter>Source Files</Filter>
-    </ClCompile>
     <ClCompile Include="TimerOne.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
     <ClCompile Include="StatusList.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="StepperControl.cpp">
+    <ClCompile Include="F09Handler.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="F09Handler.cpp">
+    <ClCompile Include="TMC2130.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="Movement.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MovementAxis.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="MovementEncoder.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_CHOPCONF.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_COOLCONF.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_DRV_STATUS.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_GCONF.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_IHOLD_IRUN.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper_PWMCONF.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="TMC2130\TMC2130Stepper.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
   </ItemGroup>
@@ -245,21 +269,12 @@
     <ClInclude Include="ServoControl.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="StepperControlAxis.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
-    <ClInclude Include="StepperControlEncoder.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="TimerOne.h">
       <Filter>Header Files</Filter>
     </ClInclude>
     <ClInclude Include="StatusList.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="StepperControl.h">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="__vm\.src.vsarduino.h">
       <Filter>Header Files</Filter>
     </ClInclude>
@@ -275,5 +290,29 @@
     <ClInclude Include="__vm\.farmbot-arduino-controller.vsarduino.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="TMC2130.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="Movement.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MovementAxis.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="MovementEncoder.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TMC2130\TMC2130Stepper_MACROS.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TMC2130\TMC2130Stepper_REGDEFS.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TMC2130\TMC2130Stepper_UTILITY.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="TMC2130\TMC2130Stepper.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>
\ No newline at end of file