From 1fa8040520ab4faf7026aeb68c19a7733cccca44 Mon Sep 17 00:00:00 2001 From: Tim Evers Date: Sun, 26 Apr 2020 20:41:30 +0200 Subject: [PATCH] Changing stall detection for express board --- src/Board.h | 4 ++-- src/F11Handler.cpp | 4 ++++ src/F12Handler.cpp | 4 ++++ src/F13Handler.cpp | 5 ++++ src/Movement.cpp | 27 ++++++++++++++++------ src/MovementAxis.cpp | 11 +++++++++ src/MovementAxis.h | 4 ++++ src/farmbot_arduino_controller.cpp | 37 +++--------------------------- 8 files changed, 53 insertions(+), 43 deletions(-) diff --git a/src/Board.h b/src/Board.h index a0cf1dd..12ea91e 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 9ed092f..2123ff8 100644 --- a/src/F11Handler.cpp +++ b/src/F11Handler.cpp @@ -35,6 +35,10 @@ int F11Handler::execute(Command *command) int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_X); int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_X); + #if defined(FARMDUINO_EXP_V20) + missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_X); + #endif + if (stepsPerMM <= 0) { missedStepsMax = 0; diff --git a/src/F12Handler.cpp b/src/F12Handler.cpp index e35d01a..4fdb19c 100644 --- a/src/F12Handler.cpp +++ b/src/F12Handler.cpp @@ -35,6 +35,10 @@ int F12Handler::execute(Command *command) int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Y); int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Y); + #if defined(FARMDUINO_EXP_V20) + missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Y); + #endif + if (stepsPerMM <= 0) { missedStepsMax = 0; diff --git a/src/F13Handler.cpp b/src/F13Handler.cpp index ab0c597..dc64347 100644 --- a/src/F13Handler.cpp +++ b/src/F13Handler.cpp @@ -37,6 +37,11 @@ int F13Handler::execute(Command *command) int stepsPerMM = ParameterList::getInstance()->getValue(MOVEMENT_STEP_PER_MM_Z); int missedStepsMax = ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_MAX_Z); + #if defined(FARMDUINO_EXP_V20) + missedStepsMax += ParameterList::getInstance()->getValue(ENCODER_MISSED_STEPS_DECAY_Z); + #endif + + if (stepsPerMM <= 0) { missedStepsMax = 0; diff --git a/src/Movement.cpp b/src/Movement.cpp index a78902e..c68fb70 100644 --- a/src/Movement.cpp +++ b/src/Movement.cpp @@ -509,6 +509,10 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS axisActive[2] = zHome; } + axisX.resetNrOfSteps(); + axisY.resetNrOfSteps(); + axisZ.resetNrOfSteps(); + axisX.checkMovement(); axisY.checkMovement(); axisZ.checkMovement(); @@ -1530,17 +1534,26 @@ void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder, stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false; standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false; - if (stallGuard || standStill) { - // In case of stall detection, count this as a missed step + // 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) + { + *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 + (*missedSteps)++; + axis->setCurrentPosition(*lastPosition); } else { - // Decrease amount of missed steps if there are no missed step - if (*missedSteps > 0) - { - (*missedSteps) -= (*encoderStepDecay); - } + // // Decrease amount of missed steps if there are no missed step + // if (*missedSteps > 0) + // { + // (*missedSteps) -= (*encoderStepDecay); + // } *lastPosition = axis->currentPosition(); encoder->setPosition(axis->currentPosition()); } diff --git a/src/MovementAxis.cpp b/src/MovementAxis.cpp index b20adb9..514ecf7 100644 --- a/src/MovementAxis.cpp +++ b/src/MovementAxis.cpp @@ -274,6 +274,11 @@ unsigned int MovementAxis::calculateSpeed(long sourcePosition, long currentPosit return newSpeed; } +void MovementAxis::resetNrOfSteps() +{ + movementSteps = 0; +} + void MovementAxis::checkAxisDirection() { @@ -400,6 +405,7 @@ void MovementAxis::setTicks() void MovementAxis::setStepAxis() { + movementSteps++; stepIsOn = true; if (movementUp) @@ -735,6 +741,11 @@ unsigned long MovementAxis::getLength(long l1, long l2) } } +unsigned long MovementAxis::getNrOfSteps() +{ + return movementSteps; +} + bool MovementAxis::endStopsReached() { return ((digitalRead(pinMin) == motorEndStopInv2) || (digitalRead(pinMax) == motorEndStopInv2)) && motorEndStopEnbl; diff --git a/src/MovementAxis.h b/src/MovementAxis.h index 20362a4..ad7da0c 100644 --- a/src/MovementAxis.h +++ b/src/MovementAxis.h @@ -89,6 +89,9 @@ public: void activateDebugPrint(); void test(); + unsigned long getNrOfSteps(); + void resetNrOfSteps(); + char channelLabel; bool movementStarted; @@ -166,6 +169,7 @@ private: unsigned long stepOnTick = 0; unsigned long stepOffTick = 0; unsigned long axisSpeed = 0; + unsigned long movementSteps = 0; bool axisActive = false; bool movementUp = false; diff --git a/src/farmbot_arduino_controller.cpp b/src/farmbot_arduino_controller.cpp index 3dbb713..4b533db 100644 --- a/src/farmbot_arduino_controller.cpp +++ b/src/farmbot_arduino_controller.cpp @@ -635,18 +635,9 @@ void startInterrupt() // Interrupt management code library written by Paul Stoffregen // The default time 100 micro seconds - //#if !defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) - Timer1.attachInterrupt(interrupt); - Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); - Timer1.start(); - //#endif - - //#if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) - // Serial.println("set timer"); - // TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow - // TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1 - // OCR2A = 4; // Set overflow to 4 for total of 64 µs - //#endif + Timer1.attachInterrupt(interrupt); + Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); + Timer1.start(); } void homeOnBoot() @@ -751,34 +742,12 @@ void startServo() #if defined(FARMDUINO_EXP_V20) || defined(FARMDUINO_V30) -/**/ -//static Trinamic_TMC2130 controllerTMC2130_X(X_CHIP_SELECT); -//static Trinamic_TMC2130 controllerTMC2130_Y(Y_CHIP_SELECT); -//static Trinamic_TMC2130 controllerTMC2130_Z(Z_CHIP_SELECT); -//static Trinamic_TMC2130 controllerTMC2130_E(E_CHIP_SELECT); - void loadTMC2130drivers() { Serial.print(COMM_REPORT_COMMENT); Serial.print(SPACE); Serial.print("Load TMC drivers"); Serial.print(CRLF); - - /**/ - //TMC2130X.init(); - //TMC2130Y.init(); - //TMC2130Z.init(); - //TMC2130E.init(); - - //Trinamic_TMC2130 controllerTMC2130_X = Trinamic_TMC2130(X_CHIP_SELECT); - //Trinamic_TMC2130 controllerTMC2130_Y = Trinamic_TMC2130(Y_CHIP_SELECT); - //Trinamic_TMC2130 controllerTMC2130_Z = Trinamic_TMC2130(Z_CHIP_SELECT); - //Trinamic_TMC2130 controllerTMC2130_E = Trinamic_TMC2130(E_CHIP_SELECT); - - //TMC2130X = &controllerTMC2130_X; - //TMC2130Y = &controllerTMC2130_Y; - //TMC2130Z = &controllerTMC2130_Z; - //TMC2130E = &controllerTMC2130_E; } void loadTMC2130parameters()