From 3d709f6618be3179ac12c1dc7a0afc215f37c6d4 Mon Sep 17 00:00:00 2001 From: Tim Evers Date: Wed, 15 Apr 2020 16:19:26 +0200 Subject: [PATCH] Homing improvements --- src/Board.h | 4 ++-- src/F11Handler.cpp | 41 ++++++++++++++++++++++++------- src/F12Handler.cpp | 43 +++++++++++++++++++++++++-------- src/F13Handler.cpp | 60 +++++++++++++++++++++++----------------------- 4 files changed, 97 insertions(+), 51 deletions(-) diff --git a/src/Board.h b/src/Board.h index 12ea91e..a0cf1dd 100644 --- a/src/Board.h +++ b/src/Board.h @@ -1,7 +1,7 @@ #ifndef FARMBOT_BOARD_ID // Farmbot using RAMPS board - //#define RAMPS_V14 + #define RAMPS_V14 //#define FARMDUINO_V10 //#define FARMDUINO_V14 @@ -10,7 +10,7 @@ //#define FARMDUINO_V30 // Farmbot Express - #define FARMDUINO_EXP_V20 + //#define FARMDUINO_EXP_V20 #else diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp index f2bead8..e4ce8e8 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -32,6 +32,9 @@ int F11Handler::execute(Command *command) } int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_X); + int stepsMerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X); + int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X); + int A = 10; // move away coordinates int execution; bool emergencyStop; @@ -49,25 +52,36 @@ int F11Handler::execute(Command *command) long Y = CurrentState::getInstance()->getY(); long Z = CurrentState::getInstance()->getZ(); - // Move to home position - Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, false, false, false); - execution = CurrentState::getInstance()->getLastError(); - emergencyStop = CurrentState::getInstance()->isEmergencyStop(); - if (emergencyStop || execution != 0) { homingComplete = true; } + bool firstMove = true; + int goodConsecutiveHomings = 0; // After the first home, keep moving away and home back // until there is no deviation in positions while (!homingComplete) { + if (firstMove) + { + firstMove = false; + + // Move to home position + Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, false, false, false); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + if (emergencyStop || execution != 0) { homingComplete = true; } + } + // Move away from the home position Movement::getInstance()->moveToCoords(A, Y, Z, 0, 0, 0, false, false, false); - execution = CurrentState::getInstance()->getLastError(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; emergencyStop = CurrentState::getInstance()->isEmergencyStop(); if (emergencyStop || execution != 0) { break; } // Home again Movement::getInstance()->moveToCoords(0, Y, Z, 0, 0, 0, true, false, false); - execution = CurrentState::getInstance()->getLastError(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; emergencyStop = CurrentState::getInstance()->isEmergencyStop(); if (emergencyStop || execution != 0) { break; } @@ -87,9 +101,18 @@ int F11Handler::execute(Command *command) Serial.print("\r\n"); // Home position cannot drift more than 5 milimeter otherwise no valid home pos - if (CurrentState::getInstance()->getHomeMissedStepsXscaled() < 5) + if (CurrentState::getInstance()->getHomeMissedStepsXscaled() < (5 + missedStepsMax * stepsMerMM)) { - homingComplete = true; + goodConsecutiveHomings++; + if (goodConsecutiveHomings >= 3) + { + homingComplete = true; + CurrentState::getInstance()->setX(0); + } + } + else + { + goodConsecutiveHomings = 0; } } diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index a11ca86..ffdf78d 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -32,6 +32,9 @@ int F12Handler::execute(Command *command) } int homeIsUp = ParameterList::getInstance()->getValue(MOVEMENT_HOME_UP_Y); + int stepsMerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y); + int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y); + int A = 10; // move away coordinates int execution; bool emergencyStop; @@ -48,27 +51,38 @@ int F12Handler::execute(Command *command) long Y = CurrentState::getInstance()->getY(); long Z = CurrentState::getInstance()->getZ(); - // Move to home position. s - Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, false, false); - execution = CurrentState::getInstance()->getLastError(); - emergencyStop = CurrentState::getInstance()->isEmergencyStop(); - if (emergencyStop || execution != 0) { homingComplete = true; } + bool firstMove = true; + int goodConsecutiveHomings = 0; // After the first home, keep moving away and home back // until there is no deviation in positions while (!homingComplete) { + if (firstMove) + { + firstMove = false; + + // Move to home position. s + Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, false, false); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + if (emergencyStop || execution != 0) { homingComplete = true; } + } + // 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(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; emergencyStop = CurrentState::getInstance()->isEmergencyStop(); if (emergencyStop || execution != 0) { break; } // Home again Movement::getInstance()->moveToCoords(X, 0, Z, 0, 0, 0, false, true, false); - execution = CurrentState::getInstance()->getLastError(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; emergencyStop = CurrentState::getInstance()->isEmergencyStop(); if (emergencyStop || execution != 0) { break; } @@ -87,10 +101,19 @@ int F12Handler::execute(Command *command) Serial.print(CurrentState::getInstance()->getHomeMissedStepsZscaled()); Serial.print("\r\n"); - // Compare postition before and after verify homing - if (CurrentState::getInstance()->getHomeMissedStepsYscaled() < 5) + // Compare postition before and after verify homing, accounting for missed steps detecting stall + if (CurrentState::getInstance()->getHomeMissedStepsYscaled() < (5 + missedStepsMax * stepsMerMM)) { - homingComplete = true; + goodConsecutiveHomings++; + if (goodConsecutiveHomings >= 3) + { + homingComplete = true; + CurrentState::getInstance()->setY(0); + } + } + else + { + goodConsecutiveHomings = 0; } } diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index 9ec5465..b4cb037 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -34,6 +34,9 @@ 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 stepsMerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z); + int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z); + int A = 10; // move away coordinates int execution; bool emergencyStop; @@ -51,25 +54,36 @@ 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; } + bool firstMove = true; + int goodConsecutiveHomings = 0; // After the first home, keep moving away and home back // until there is no deviation in positions while (!homingComplete) { + if (firstMove) + { + firstMove = false; + + // Move to home position. + Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, false); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; + emergencyStop = CurrentState::getInstance()->isEmergencyStop(); + if (emergencyStop || execution != 0) { homingComplete = true; } + } + // Move away from the home position Movement::getInstance()->moveToCoords(X, Y, A, 0, 0, 0, false, false, false); - execution = CurrentState::getInstance()->getLastError(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; 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(); + //execution = CurrentState::getInstance()->getLastError(); + execution = 0; emergencyStop = CurrentState::getInstance()->isEmergencyStop(); if (emergencyStop || execution != 0) { break; } @@ -89,32 +103,18 @@ int F13Handler::execute(Command *command) Serial.print("\r\n"); // Compare postition before and after verify homing - if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < 5) + if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < (5 + missedStepsMax * stepsMerMM)) { - homingComplete = true; + goodConsecutiveHomings++; + if (goodConsecutiveHomings >= 3) + { + homingComplete = true; + CurrentState::getInstance()->setZ(0); + } } - } - - // Move to home position. Then 3 times move away and move to home again. - for (int stepNr = 0; stepNr < 7; stepNr++) - { - switch (stepNr) + else { - case 0: break; - case 1: 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; - case 6: Movement::getInstance()->moveToCoords(X, Y, 0, 0, 0, 0, false, false, true); break; - } - - execution = CurrentState::getInstance()->getLastError(); - emergencyStop = CurrentState::getInstance()->isEmergencyStop(); - - if (emergencyStop || execution != 0) - { - break; + goodConsecutiveHomings = 0; } }