TMC2130 tests

2019-12-02 22:07:28 +01:00 · 2019-12-02 22:07:28 +01:00 · 92f0e7dd94
parent 8c5d0385f1
commit 92f0e7dd94
57 changed files with 1685 additions and 866 deletions
--- a/CppProperties.json
+++ 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"
+    }
+  ]
+}
--- 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'};
--- 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;
 }
--- 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;

--- 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();
--- 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
 {
--- 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();
--- 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
 {
--- 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();
--- 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
 {
--- 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);
 	}
 	

--- 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
 {
--- 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)
--- 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
 {
--- 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);
  }


--- 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
 {
--- 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
 {
--- 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
--- 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
--- 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
--- 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
--- 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
--- 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
 {
--- 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
 {
--- 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();
--- 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
 {
--- 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);
--- 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
 {
--- 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)
  {
--- 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
 {
--- a/src/StepperControl.cpp
+++ b/src/StepperControl.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
--- a/src/StepperControl.h
+++ b/src/StepperControl.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_ */
--- a/src/StepperControlAxis.cpp
+++ b/src/StepperControlAxis.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
 }
--- a/src/StepperControlAxis.h
+++ b/src/StepperControlAxis.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_ */
--- a/src/StepperControlEncoder.cpp
+++ b/src/StepperControlEncoder.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)
--- a/src/StepperControlEncoder.h
+++ b/src/StepperControlEncoder.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_ */
--- a/src/README.md
+++ b/src/README.md
@ -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/
--- a/src/TMC2130.cpp
+++ 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;
+}
+*/
--- a/src/TMC2130.h
+++ 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_ */
--- a/src/TMC2130_Backup/SW_SPI.cpp
+++ b/src/TMC2130_Backup/SW_SPI.cpp
--- a/src/TMC2130_Backup/SW_SPI.h
+++ b/src/TMC2130_Backup/SW_SPI.h
--- a/src/TMC2130_Backup/TMC2130Stepper.cpp
+++ b/src/TMC2130_Backup/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() {
--- a/src/TMC2130_Backup/TMC2130Stepper.h
+++ b/src/TMC2130_Backup/TMC2130Stepper.h
--- a/src/TMC2130_Backup/TMC2130Stepper_CHOPCONF.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_CHOPCONF.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_COOLCONF.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_COOLCONF.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_DRV_STATUS.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_DRV_STATUS.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_GCONF.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_GCONF.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_IHOLD_IRUN.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_IHOLD_IRUN.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_MACROS.h
+++ b/src/TMC2130_Backup/TMC2130Stepper_MACROS.h
--- a/src/TMC2130_Backup/TMC2130Stepper_PWMCONF.cpp
+++ b/src/TMC2130_Backup/TMC2130Stepper_PWMCONF.cpp
--- a/src/TMC2130_Backup/TMC2130Stepper_REGDEFS.h
+++ b/src/TMC2130_Backup/TMC2130Stepper_REGDEFS.h
--- a/src/TMC2130_Backup/TMC2130Stepper_UTILITY.h
+++ b/src/TMC2130_Backup/TMC2130Stepper_UTILITY.h
--- a/src/farmbot_arduino_controller.cpp
+++ b/src/farmbot_arduino_controller.cpp
--- 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_ */
--- 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();
+
+}
--- 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>
--- 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>