Homing changes for verification of home position

src/CurrentState.cpp

@@ -8,13 +8,6 @@
 #include "CurrentState.h"
 
 static CurrentState *instance;
-long x = 0;
-long y = 0;
-long z = 0;
-unsigned int speed = 0;
-bool endStopState[3][2];
-long Q = 0;
-int lastError = 0;
 
 CurrentState *CurrentState::getInstance()
 {
@@ -30,6 +23,11 @@ CurrentState::CurrentState()
   x = 0;
   y = 0;
   z = 0;
+
+  homeMissedStepX = 0;
+  homeMissedStepY = 0;
+  homeMissedStepZ = 0;
+
   speed = 0;
   Q = 0;
   lastError = 0;
@@ -71,6 +69,54 @@ void CurrentState::setZ(long newZ)
   z = newZ;
 }
 
+long CurrentState::getHomeMissedStepsX()
+{
+  return homeMissedStepX;
+}
+
+long CurrentState::getHomeMissedStepsXscaled()
+{
+  return homeMissedStepX / stepsPerMmX;
+}
+
+long CurrentState::getHomeMissedStepsY()
+{
+  return homeMissedStepY;
+}
+
+long CurrentState::getHomeMissedStepsYscaled()
+{
+  return homeMissedStepY / stepsPerMmY;
+}
+
+long CurrentState::getHomeMissedStepsZ()
+{
+  return homeMissedStepZ;
+}
+
+long CurrentState::getHomeMissedStepsZscaled()
+{
+  return homeMissedStepZ / stepsPerMmZ;
+}
+
+void CurrentState::setHomeMissedStepsX(long newX)
+{
+  homeMissedStepX = newX;
+}
+
+void CurrentState::setHomeMissedStepsY(long newY)
+{
+  homeMissedStepY = newY;
+}
+
+void CurrentState::setHomeMissedStepsZ(long newZ)
+{
+  homeMissedStepZ = newZ;
+}
+
+
+
+
 int CurrentState::getLastError()
 {
   return lastError;
@@ -93,6 +139,21 @@ void CurrentState::setStepsPerMm(long stepsX, long stepsY, long stepsZ)
   stepsPerMmZ = stepsZ;
 }
 
+long CurrentState::getStepsPerMmX()
+{
+  return stepsPerMmX;
+}
+
+long CurrentState::getStepsPerMmY()
+{
+  return stepsPerMmY;
+}
+
+long CurrentState::getStepsPerMmZ()
+{
+  return stepsPerMmZ;
+}
+
 void CurrentState::storeEndStops()
 {
   CurrentState::getInstance()->setEndStopState(0, 0, digitalRead(X_MIN_PIN));

src/CurrentState.h

@@ -36,6 +36,17 @@ public:
   void setY(long);
   void setZ(long);
 
+  long getHomeMissedStepsX();
+  long getHomeMissedStepsY();
+  long getHomeMissedStepsZ();
+  long getHomeMissedStepsXscaled();
+  long getHomeMissedStepsYscaled();
+  long getHomeMissedStepsZscaled();
+  void setHomeMissedStepsX(long);
+  void setHomeMissedStepsY(long);
+  void setHomeMissedStepsZ(long);
+
+
   int getLastError();
   void setLastError(int error);
 
@@ -57,6 +68,10 @@ public:
   bool isEmergencyStop();
 
   void setStepsPerMm(long stepsX, long stepsY, long stepsZ);
+  long getStepsPerMmX();
+  long getStepsPerMmY();
+  long getStepsPerMmZ();
+
 
 private:
   CurrentState();
@@ -69,6 +84,19 @@ private:
 
   int errorCode = 0;
 
+  long x = 0;
+  long y = 0;
+  long z = 0;
+  unsigned int speed = 0;
+  bool endStopState[3][2];
+  long Q = 0;
+  int lastError = 0;
+
+  long homeMissedStepX = 0;
+  long homeMissedStepY = 0;
+  long homeMissedStepZ = 0;
+
+
   bool emergencyStop = false;
 };
 

src/F11Handler.cpp

@@ -32,10 +32,12 @@ int F11Handler::execute(Command *command)
   }
 
   int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X);
-  int A = 10; // move away coordinates
+  int A = 10 * CurrentState::getInstance()->getStepsPerMmX(); // move away coordinates
   int execution;
   bool emergencyStop;
 
+  bool homingComplete = false;
+
   if (homeIsUp == 1)
   {
     A = -A;
@@ -47,26 +49,32 @@ int F11Handler::execute(Command *command)
   long Y = CurrentState::getInstance()->getY();
   long Z = CurrentState::getInstance()->getZ();
 
-  // Move to home position. Then 3 times move away and move to home again.
+  // Move to home position
-  for (int stepNr = 0; stepNr < 7; stepNr++)
+  Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, false, false, false);
-  {
+  execution = CurrentState::getInstance()->getLastError();
-    switch (stepNr)
+  emergencyStop = CurrentState::getInstance()->isEmergencyStop();
-    {
+  if (emergencyStop || execution != 0) { homingComplete = true; }
-      case 0: Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false); break;
-      case 1: Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false); break;
-      case 2: Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false); break;
-      case 3: Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false); break;
-      case 4: Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false); break;
-      case 5: Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false); break;
-      case 6: Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false); break;
-    }
 
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+    // Move away from the home position
+    Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false);
     execution = CurrentState::getInstance()->getLastError();
     emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
 
-    if (emergencyStop || execution != 0)
+    // Home again
+    Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false);
+    execution = CurrentState::getInstance()->getLastError();
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Home position cannot drift more than 5 milimeter otherwise no valid home pos
+    if (CurrentState::getInstance()->getHomeMissedStepsXscaled() < 5)
     {
-      break;
+      homingComplete = true;
     }
   }
 
@@ -74,5 +82,6 @@ int F11Handler::execute(Command *command)
   {
     CurrentState::getInstance()->print();
   }
+
   return 0;
 }

src/F12Handler.cpp

@@ -32,9 +32,10 @@ int F12Handler::execute(Command *command)
   }
 
   int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y);
-  int A = 10; // move away coordinates
+  int A = 10 * CurrentState::getInstance()->getStepsPerMmY(); // move away coordinates
   int execution;
   bool emergencyStop;
+  bool homingComplete = false;
 
   if (homeIsUp == 1)
   {
@@ -47,26 +48,34 @@ int F12Handler::execute(Command *command)
   long Y = CurrentState::getInstance()->getY();
   long Z = CurrentState::getInstance()->getZ();
 
-  // Move to home position. Then 3 times move away and move to home again.
+  // Move to home position. s
-  for (int stepNr = 0; stepNr < 7; stepNr++)
+  Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, false, false);
-  {
+  execution = CurrentState::getInstance()->getLastError();
-    switch (stepNr)
+  emergencyStop = CurrentState::getInstance()->isEmergencyStop();
-    {
+  if (emergencyStop || execution != 0) { homingComplete = true; }
-      case 0: Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false); break;
-      case 1: Movement::getInstance()->moveToCoords(X, A, Z, 0, 0, 0, false, false, false); break;
-      case 2: Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false); break;
-      case 3: Movement::getInstance()->moveToCoords(X, A, Z, 0, 0, 0, false, false, false); break;
-      case 4: Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false); break;
-      case 5: Movement::getInstance()->moveToCoords(X, A, Z, 0, 0, 0, false, false, false); break;
-      case 6: Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false); break;
-    }
 
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+
+    // Move away from the home position
+    //posBeforeVerify = CurrentState::getInstance()->getY();
+    Movement::getInstance()->moveToCoords(X, A, Z, 0, 0, 0, false, false, false);
     execution = CurrentState::getInstance()->getLastError();
     emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
 
-    if (emergencyStop || execution != 0)
+    // Home again
+    Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false);
+    execution = CurrentState::getInstance()->getLastError();
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Compare postition before and after verify homing
+    if (CurrentState::getInstance()->getHomeMissedStepsYscaled() < 5)
     {
-      break;
+      homingComplete = true;
     }
   }
 

src/F13Handler.cpp

@@ -34,10 +34,12 @@ int F13Handler::execute(Command *command)
   Movement::getInstance()->moveToCoords(0, 0, 0, 0, 0, 0, false, false, true);
 
   int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Z);
-  int A = 10; // move away coord
+  int A = 10 * CurrentState::getInstance()->getStepsPerMmZ(); // move away coordinates
   int execution;
   bool emergencyStop;
 
+  bool homingComplete = false;
+
   if (homeIsUp == 1)
   {
     A = -A;
@@ -49,14 +51,43 @@ int F13Handler::execute(Command *command)
   long Y = CurrentState::getInstance()->getY();
   long Z = CurrentState::getInstance()->getZ();
 
+  // Move to home position. 
+  Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, false);
+  execution = CurrentState::getInstance()->getLastError();
+  emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+  if (emergencyStop || execution != 0) { homingComplete = true; }
+
+  // After the first home, keep moving away and home back
+  // until there is no deviation in positions
+  while (!homingComplete)
+  {
+    // Move away from the home position
+    Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false);
+    execution = CurrentState::getInstance()->getLastError();
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Home again
+    Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true);
+    execution = CurrentState::getInstance()->getLastError();
+    emergencyStop = CurrentState::getInstance()->isEmergencyStop();
+    if (emergencyStop || execution != 0) { break; }
+
+    // Compare postition before and after verify homing
+    if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < 5)
+    {
+      homingComplete = true;
+    }
+  }
+
   // Move to home position. Then 3 times move away and move to home again.
   for (int stepNr = 0; stepNr < 7; stepNr++)
   {
     switch (stepNr)
     {
-    case 0: Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true); break;
+    case 0:  break;
-    case 1: Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false); break;
+    case 1:  break;
-    case 2: Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true); break;
+    case 2: 
     case 3: Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false); break;
     case 4: Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true); break;
     case 5: Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false); break;

src/Movement.cpp

@@ -426,6 +426,9 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
   destinationPoint[1] = yDest;
   destinationPoint[2] = zDest;
 
+  long homeMissedSteps[3] = { 0, 0, 0 };
+
+
   motorConsMissedSteps[0] = 0;
   motorConsMissedSteps[1] = 0;
   motorConsMissedSteps[2] = 0;
@@ -557,6 +560,20 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
       axisX.checkMovement();
       checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
       axisX.resetStepDone();
+
+      // While homing and being at the place where home is supposed to be
+      // start counting how many steps are taken before the motor is deactivated
+      if (
+          xHome == true &&
+          (
+            (homeIsUp[0] == false && currentPoint[0] <= 0) ||
+            (homeIsUp[0] == true  && currentPoint[0] >= 0)
+          )
+        )
+      {
+        homeMissedSteps[0]++;
+      }
+
     }
 
     if (axisY.isStepDone())
@@ -564,6 +581,20 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
       axisY.checkMovement();
       checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]);
       axisY.resetStepDone();
+
+      // While homing and being at the place where home is supposed to be
+      // start counting how many steps are taken before the motor is deactivated
+      if (
+          yHome == true &&
+          (
+            (homeIsUp[1] == false && currentPoint[1] <= 0) ||
+            (homeIsUp[1] == true  && currentPoint[1] >= 0)
+          )
+        )
+      {
+        homeMissedSteps[1]++;
+      }
+
     }
 
     if (axisZ.isStepDone())
@@ -571,6 +602,20 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
       axisZ.checkMovement();
       checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]);
       axisZ.resetStepDone();
+
+      // While homing and being at the place where home is supposed to be
+      // start counting how many steps are taken before the motor is deactivated
+      if (
+            zHome == true &&
+            (
+              (homeIsUp[2] == false && currentPoint[2] <= 0) ||
+              (homeIsUp[2] == true  && currentPoint[2] >= 0)
+            )
+        )
+      {
+        homeMissedSteps[2]++;
+      }
+
     }
 
     if (axisX.isAxisActive() && motorConsMissedSteps[0] > motorConsMissedStepsMax[0])
@@ -842,6 +887,10 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
   CurrentState::getInstance()->setY(currentPoint[1]);
   CurrentState::getInstance()->setZ(currentPoint[2]);
 
+  CurrentState::getInstance()->setHomeMissedStepsX(homeMissedSteps[0]);
+  CurrentState::getInstance()->setHomeMissedStepsY(homeMissedSteps[1]);
+  CurrentState::getInstance()->setHomeMissedStepsZ(homeMissedSteps[2]);
+
   storeEndStops();
   reportEndStops();
   reportPosition();