From 79d5d30f01796f37bf6acdaa415c3c0b2a926e5b Mon Sep 17 00:00:00 2001 From: Tim Evers Date: Tue, 12 May 2020 21:36:49 +0200 Subject: [PATCH] Changes to missed step detection --- src/F11Handler.cpp | 2 +- src/F12Handler.cpp | 12 +- src/F13Handler.cpp | 2 +- src/Movement.cpp | 286 +++++++++++++++++++++++++++++++------------ src/Movement.h | 3 + src/MovementAxis.cpp | 67 ++++++++++ src/MovementAxis.h | 12 ++ 7 files changed, 300 insertions(+), 84 deletions(-) diff --git a/src/F11Handler.cpp b/src/F11Handler.cpp index 2123ff8..77bcb17 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -111,7 +111,7 @@ 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 + missedStepsMax / stepsPerMM)) + if (CurrentState::getInstance()->getHomeMissedStepsXscaled() < (20 + (missedStepsMax * 3) / stepsPerMM)) { goodConsecutiveHomings++; if (goodConsecutiveHomings >= 3) diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index 4fdb19c..5ab7926 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -98,21 +98,19 @@ int F12Handler::execute(Command *command) Serial.print("R99 homing displaced"); Serial.print(" X "); - Serial.print(CurrentState::getInstance()->getHomeMissedStepsX()); - Serial.print(" / "); Serial.print(CurrentState::getInstance()->getHomeMissedStepsXscaled()); Serial.print(" Y "); - Serial.print(CurrentState::getInstance()->getHomeMissedStepsY()); - Serial.print(" / "); Serial.print(CurrentState::getInstance()->getHomeMissedStepsYscaled()); Serial.print(" Z "); - Serial.print(CurrentState::getInstance()->getHomeMissedStepsZ()); - Serial.print(" / "); Serial.print(CurrentState::getInstance()->getHomeMissedStepsZscaled()); + Serial.print(" M "); + Serial.print(missedStepsMax); + Serial.print(" M "); + Serial.print(stepsPerMM); Serial.print("\r\n"); // Compare postition before and after verify homing, accounting for missed steps detecting stall - if (CurrentState::getInstance()->getHomeMissedStepsYscaled() < (5 + missedStepsMax / stepsPerMM)) + if (CurrentState::getInstance()->getHomeMissedStepsYscaled() <= (20 + (missedStepsMax * 3) / stepsPerMM)) { goodConsecutiveHomings++; if (goodConsecutiveHomings >= 3) diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index dc64347..d3c4d6a 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -114,7 +114,7 @@ int F13Handler::execute(Command *command) Serial.print("\r\n"); // Compare postition before and after verify homing - if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < (5 + missedStepsMax / stepsPerMM)) + if (CurrentState::getInstance()->getHomeMissedStepsZscaled() < (20 + (missedStepsMax * 3) / stepsPerMM)) { goodConsecutiveHomings++; if (goodConsecutiveHomings >= 3) diff --git a/src/Movement.cpp b/src/Movement.cpp index 7d6ede5..9297ff1 100644 --- a/src/Movement.cpp +++ b/src/Movement.cpp @@ -218,68 +218,88 @@ void Movement::loadSettings() void Movement::loadSettingsTMC2130() { + loadSettingsTMC2130_X(); + loadSettingsTMC2130_Y(); + loadSettingsTMC2130_Z(); + /**/ + //int motorCurrentX; + //int stallSensitivityX; + //int microStepsX; + + //int motorCurrentY; + //int stallSensitivityY; + //int microStepsY; + + //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); + + //motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y); + //stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y); + //microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y); + + //motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z); + //stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z); + //microStepsZ = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z); + + //if (microStepsX <= 0) { microStepsX = 1; } + //if (microStepsY <= 0) { microStepsY = 1; } + //if (microStepsZ <= 0) { microStepsZ = 1; } + + + //axisX.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); + //axisY.loadSettingsTMC2130(motorCurrentY, stallSensitivityY, microStepsY); + //axisZ.loadSettingsTMC2130(motorCurrentZ, stallSensitivityZ, microStepsZ); + } + + void Movement::loadSettingsTMC2130_X() + { int motorCurrentX; int stallSensitivityX; int microStepsX; - int motorCurrentY; - int stallSensitivityY; - int microStepsY; - - 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); + if (microStepsX <= 0) { microStepsX = 1; } + + + axisX.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); + } + + void Movement::loadSettingsTMC2130_Y() + { + int motorCurrentY; + int stallSensitivityY; + int microStepsY; + motorCurrentY = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Y); stallSensitivityY = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Y); microStepsY = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Y); + if (microStepsY <= 0) { microStepsY = 1; } + + axisY.loadSettingsTMC2130(motorCurrentY, stallSensitivityY, microStepsY); + } + + void Movement::loadSettingsTMC2130_Z() + { + int motorCurrentZ; + int stallSensitivityZ; + int microStepsZ; + motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z); stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z); microStepsZ = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z); - /**/ - - //Serial.print("=X=>"); - //Serial.print(" "); - //Serial.print(microStepsX); - //Serial.print(" "); - //Serial.print(motorCurrentX); - //Serial.print(" "); - //Serial.print(stallSensitivityX); - //Serial.println(" "); - - //Serial.print("=Y=>"); - //Serial.print(" "); - //Serial.print(microStepsY); - //Serial.print(" "); - //Serial.print(motorCurrentY); - //Serial.print(" "); - //Serial.print(stallSensitivityY); - //Serial.println(" "); - - //Serial.print("=Z=>"); - //Serial.print(" "); - //Serial.print(microStepsZ); - //Serial.print(" "); - //Serial.print(motorCurrentZ); - //Serial.print(" "); - //Serial.print(stallSensitivityZ); - //Serial.println(" "); - - - if (microStepsX <= 0) { microStepsX = 1; } - if (microStepsY <= 0) { microStepsY = 1; } if (microStepsZ <= 0) { microStepsZ = 1; } - - axisX.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); - axisY.loadSettingsTMC2130(motorCurrentY, stallSensitivityY, microStepsY); axisZ.loadSettingsTMC2130(motorCurrentZ, stallSensitivityZ, microStepsZ); } @@ -621,11 +641,30 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS } - if (axisX.isAxisActive() && motorConsMissedSteps[0] > motorConsMissedStepsMax[0]) + #if defined(FARMDUINO_EXP_V20) + if (axisX.isAxisActive() && + axisX.missedStepHistory[0] >= motorConsMissedStepsMax[0] && + axisX.missedStepHistory[1] >= motorConsMissedStepsMax[0] && + axisX.missedStepHistory[2] >= motorConsMissedStepsMax[0] && + axisX.missedStepHistory[3] >= motorConsMissedStepsMax[0] && + axisX.missedStepHistory[4] >= motorConsMissedStepsMax[0] ) + #else + if (axisX.isAxisActive() && motorConsMissedSteps[0] >= motorConsMissedStepsMax[0]) + #endif { axisX.deactivateAxis(); + serialBuffer += "R99"; - serialBuffer += " deactivate motor X due to missed steps"; + serialBuffer += " deactivate motor X due to "; + if (axisX.isDriverError()) + { + serialBuffer += "driver error"; + } + else + { + + serialBuffer += "missed steps"; + } serialBuffer += "\r\n"; if (xHome) @@ -639,13 +678,41 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS } } - if (axisY.isAxisActive() && motorConsMissedSteps[1] > motorConsMissedStepsMax[1]) + #if defined(FARMDUINO_EXP_V20) + if (axisY.isAxisActive() && + axisY.missedStepHistory[0] >= motorConsMissedStepsMax[1] && + axisY.missedStepHistory[1] >= motorConsMissedStepsMax[1] && + axisY.missedStepHistory[2] >= motorConsMissedStepsMax[1] && + axisY.missedStepHistory[3] >= motorConsMissedStepsMax[1] && + axisY.missedStepHistory[4] >= motorConsMissedStepsMax[1]) + #else + if (axisY.isAxisActive() && motorConsMissedSteps[1] >= motorConsMissedStepsMax[1]) + #endif { axisY.deactivateAxis(); + serialBuffer += "R99"; - serialBuffer += " deactivate motor Y due to missed steps"; + serialBuffer += " deactivate motor Y due to "; + if (axisY.isDriverError()) + { + serialBuffer += "driver error"; + } + else + { + + serialBuffer += "missed steps"; + } serialBuffer += "\r\n"; - + + serialBuffer += "R99"; + serialBuffer += " S"; + serialBuffer += axisY.getNrOfSteps(); + serialBuffer += " MS "; + serialBuffer += motorConsMissedSteps[1]; + serialBuffer += " MSM "; + serialBuffer += motorConsMissedStepsMax[1]; + serialBuffer += "\r\n"; + if (yHome) { encoderY.setPosition(0); @@ -657,12 +724,30 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS } } - if (axisZ.isAxisActive() && motorConsMissedSteps[2] > motorConsMissedStepsMax[2]) + #if defined(FARMDUINO_EXP_V20) + if (axisZ.isAxisActive() && + axisZ.missedStepHistory[0] >= motorConsMissedStepsMax[2] && + axisZ.missedStepHistory[1] >= motorConsMissedStepsMax[2] && + axisZ.missedStepHistory[2] >= motorConsMissedStepsMax[2] && + axisZ.missedStepHistory[3] >= motorConsMissedStepsMax[2] && + axisZ.missedStepHistory[4] >= motorConsMissedStepsMax[2]) + #else + if (axisZ.isAxisActive() && motorConsMissedSteps[2] >= motorConsMissedStepsMax[2]) + #endif { axisZ.deactivateAxis(); serialBuffer += "R99"; - serialBuffer += " deactivate motor Z due to missed steps"; + serialBuffer += " deactivate motor Z due to "; + if (axisZ.isDriverError()) + { + serialBuffer += "driver error"; + } + else + { + + serialBuffer += "missed steps"; + } serialBuffer += "\r\n"; if (zHome) @@ -911,6 +996,36 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS disableMotors(); + #if defined(FARMDUINO_EXP_V20) + if (axisX.isDriverError()) + { + Serial.print("R99 reset motor X\r\n"); + delay(100); + axisX.initTMC2130(); + loadSettingsTMC2130_X(); + delay(100); + } + + if (axisY.isDriverError()) + { + Serial.print("R99 reset motor Y\r\n"); + delay(100); + axisY.initTMC2130(); + loadSettingsTMC2130_Y(); + delay(100); + } + + if (axisZ.isDriverError()) + { + Serial.print("R99 reset motor Z\r\n"); + delay(100); + axisZ.initTMC2130(); + loadSettingsTMC2130_Z(); + delay(100); + } + + #endif + if (emergencyStop) { CurrentState::getInstance()->setEmergencyStop(); @@ -1525,52 +1640,71 @@ void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, /**/ bool stallGuard = false; bool standStill = false; + bool driverError = false; + uint8_t status = 0; + axis->readDriverStatus(); + if (*encoderEnabled) { - status = axis->getStatus(); + //status = axis->getStatus(); + //stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false; + //standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false; + //driverError = status & FB_TMC_SPISTATUS_ERROR_MASK ? true : false; - stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false; - standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false; + stallGuard = axis->isStallGuard(); + standStill = axis->isStandStill(); + driverError = axis->isDriverError(); + + //if (driverError) + //{ + // Serial.println("R99 DRIVER ERROR !!**"); + //} + + + //if (standStill) + //{ + // Serial.println("R99 STANDSTILL !!**"); + //} // Reset every hunderd steps, so the missed steps represents the number of missed steps // out of every hundred steps done if (axis->getNrOfSteps() % 100 == 0) { + // save the history + axis->missedStepHistory[4] = axis->missedStepHistory[3]; + axis->missedStepHistory[3] = axis->missedStepHistory[2]; + axis->missedStepHistory[2] = axis->missedStepHistory[1]; + axis->missedStepHistory[1] = axis->missedStepHistory[0]; + axis->missedStepHistory[0] = *missedSteps; + + // test print + Serial.print("R99"); + Serial.print(" "); + Serial.print("S = "); + Serial.print(" "); + Serial.print(axis->getNrOfSteps()); + Serial.print(" "); + Serial.print("M = "); + Serial.print(" "); + Serial.print(*missedSteps); + Serial.print("\r\n"); + + // reset missed steps *missedSteps = 0; } - if (stallGuard || standStill && axis->getNrOfSteps() >= *encoderStepDecay) { - // In case of stall detection, count this as a missed step. But ignore the first 500 steps + if ((stallGuard || standStill || driverError) && axis->getNrOfSteps() >= *encoderStepDecay) { + // In case of stall detection, count this as a missed step. But ignore the first steps (*missedSteps)++; - - //axis->setCurrentPosition(*lastPosition); } - //else { - // // Decrease amount of missed steps if there are no missed step - // if (*missedSteps > 0) - // { - // (*missedSteps) -= (*encoderStepDecay); - // } - // *lastPosition = axis->currentPosition(); - // encoder->setPosition(axis->currentPosition()); - //} *lastPosition = axis->currentPosition(); 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 @@ -1700,6 +1834,7 @@ void Movement::loadEncoderSettings() motorConsMissedStepsDecay[1] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y); motorConsMissedStepsDecay[2] = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z); +#if !defined(FARMDUINO_EXP_V20) motorConsMissedStepsDecay[0] = motorConsMissedStepsDecay[0] / 100; motorConsMissedStepsDecay[1] = motorConsMissedStepsDecay[1] / 100; motorConsMissedStepsDecay[2] = motorConsMissedStepsDecay[2] / 100; @@ -1707,6 +1842,7 @@ void Movement::loadEncoderSettings() motorConsMissedStepsDecay[0] = min(max(motorConsMissedStepsDecay[0], 0.01), 99); motorConsMissedStepsDecay[1] = min(max(motorConsMissedStepsDecay[1], 0.01), 99); motorConsMissedStepsDecay[2] = min(max(motorConsMissedStepsDecay[2], 0.01), 99); +#endif motorConsEncoderType[0] = ParameterList::getInstance()->getValue(ENCODER_TYPE_X); motorConsEncoderType[1] = ParameterList::getInstance()->getValue(ENCODER_TYPE_Y); diff --git a/src/Movement.h b/src/Movement.h index b620b24..d8cdb87 100644 --- a/src/Movement.h +++ b/src/Movement.h @@ -40,6 +40,9 @@ public: #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) void initTMC2130(); void loadSettingsTMC2130(); + void loadSettingsTMC2130_X(); + void loadSettingsTMC2130_Y(); + void loadSettingsTMC2130_Z(); #endif diff --git a/src/MovementAxis.cpp b/src/MovementAxis.cpp index 514ecf7..ef14202 100644 --- a/src/MovementAxis.cpp +++ b/src/MovementAxis.cpp @@ -128,7 +128,74 @@ void MovementAxis::loadSettingsTMC2130(int motorCurrent, int stallSensitivity, } } +void MovementAxis::readDriverStatus() +{ + TMC2130A->read_REG(FB_TMC_REG_DRV_STATUS, &driverStatus); +} + +bool MovementAxis::isStallGuard() +{ + return ((driverStatus & 0x01000000) != 0); +} + +bool MovementAxis::isStandStill() +{ + return ((driverStatus & 0x80000000) != 0); +} + +bool MovementAxis::isDriverError() +{ + //uint32_t x = driverStatus; + //x = x >> 24; + + //if (x > 1) + //{ + // Serial.print("R99"); + // Serial.print(" "); + // Serial.print("x ="); + // Serial.print(""); + // Serial.print(x); + // Serial.print("\r\n"); + //} + + return ((driverStatus & 0x7E000000) != 0); +} + uint8_t MovementAxis::getStatus() { + + uint32_t gstat = 0; + TMC2130A->read_REG(FB_TMC_REG_GSTAT, &gstat); + + uint32_t drvStat = 0; + TMC2130A->read_REG(FB_TMC_REG_DRV_STATUS, &drvStat); + + + //bool drvErr = false; + + //dr = gstat & FB_TMC_ ? true : false; + + if (gstat > 0) + { + Serial.print("R99"); + Serial.print(" "); + Serial.print(" DRV ERR"); + Serial.print(" "); + Serial.print(gstat); + Serial.print("\r\n "); + } + + drvStat = drvStat & 0xFF000000; + + if (drvStat > 0) + { + Serial.print("R99"); + Serial.print(" "); + Serial.print(" DRV STAT"); + Serial.print(" "); + Serial.print(drvStat); + Serial.print("\r\n "); + } + //return TMC2130X.read_STAT(); return TMC2130A->read_STAT(); } diff --git a/src/MovementAxis.h b/src/MovementAxis.h index ad7da0c..74070f6 100644 --- a/src/MovementAxis.h +++ b/src/MovementAxis.h @@ -100,6 +100,7 @@ public: void loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps); uint16_t getLoad(); + #endif #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) @@ -110,8 +111,15 @@ public: uint8_t getStatus(); + void readDriverStatus(); + bool isStallGuard(); + bool isStandStill(); + bool isDriverError(); + unsigned int getLostSteps(); + int missedStepHistory[5] = {0,0,0,0,0}; + bool tmcStep = true; bool tmcStep2 = true; #endif @@ -120,6 +128,10 @@ private: int lastCalcLog = 0; bool debugPrint = false; +#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) + uint32_t driverStatus = 0; +#endif + // pin settings primary motor int pinStep; int pinDirection;