diff --git a/src/Movement.cpp b/src/Movement.cpp index 979e823..a12d03f 100644 --- a/src/Movement.cpp +++ b/src/Movement.cpp @@ -1233,10 +1233,34 @@ int Movement::calibrateAxis(int axis) unsigned long nextTick = 0; unsigned long lastTick = 0; +#if defined(FARMDUINO_EXP_V20) + axisX.missedStepHistory[0] = 0; + axisX.missedStepHistory[1] = 0; + axisX.missedStepHistory[2] = 0; + axisX.missedStepHistory[3] = 0; + axisX.missedStepHistory[4] = 0; + + axisY.missedStepHistory[0] = 0; + axisY.missedStepHistory[1] = 0; + axisY.missedStepHistory[2] = 0; + axisY.missedStepHistory[3] = 0; + axisY.missedStepHistory[4] = 0; + + axisZ.missedStepHistory[0] = 0; + axisZ.missedStepHistory[1] = 0; + axisZ.missedStepHistory[2] = 0; + axisZ.missedStepHistory[3] = 0; + axisZ.missedStepHistory[4] = 0; +#endif + // Prepare for movement tickDelay = (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / speedHome[axis] / 2); + #if defined(FARMDUINO_EXP_V20) + stepDelay = 1000000 / speedHome[axis] / 4; + #else stepDelay = 100000 / speedHome[axis] / 2; + #endif Serial.print("R99"); Serial.print(" "); @@ -1420,17 +1444,43 @@ int Movement::calibrateAxis(int axis) /**/ //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) + + + //Serial.print("R99 missed steps max "); + //Serial.print(*missedStepsMax); + //Serial.print(" * "); + //Serial.print(calibAxis->missedStepHistory[0]); + //Serial.print(" "); + //Serial.print(calibAxis->missedStepHistory[1]); + //Serial.print(" "); + //Serial.print(calibAxis->missedStepHistory[2]); + //Serial.print(" "); + //Serial.print(calibAxis->missedStepHistory[3]); + //Serial.print(" "); + //Serial.print(calibAxis->missedStepHistory[4]); + //Serial.print(" "); + //Serial.print("\r\n"); + +#if defined(FARMDUINO_EXP_V20) + if ( + (!calibAxis->endStopMin() && !calibAxis->endStopMax()) && + !movementDone + && + !( + calibAxis->missedStepHistory[0] >= *missedStepsMax && + calibAxis->missedStepHistory[1] >= *missedStepsMax && + calibAxis->missedStepHistory[2] >= *missedStepsMax && + calibAxis->missedStepHistory[3] >= *missedStepsMax && + calibAxis->missedStepHistory[4] >= *missedStepsMax + ) + ) +#else if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) +#endif { calibAxis->setStepAxis(); - - lastTick = calibrationTicks; - nextTick = calibrationTicks + tickDelay; - while (calibrationTicks < nextTick && calibrationTicks >= lastTick) - { - delayMicroseconds(250); - } + delayMicroseconds(stepDelay); stepsCount++; if (stepsCount % (speedHome[axis] * 3) == 0) @@ -1461,13 +1511,7 @@ int Movement::calibrateAxis(int axis) calibAxis->resetMotorStep(); - //delayMicroseconds(stepDelay); - lastTick = calibrationTicks; - nextTick = calibrationTicks + tickDelay; - while (calibrationTicks < nextTick && calibrationTicks >= lastTick) - { - delayMicroseconds(250); - } + delayMicroseconds(stepDelay); } else { @@ -1615,8 +1659,21 @@ int Movement::calibrateAxis(int axis) /**/ //if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax)) //if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax)) +#if defined(FARMDUINO_EXP_V20) + if ( + ((!invertEndStops && !calibAxis->endStopMin()) || (invertEndStops && !calibAxis->endStopMax())) && + !movementDone && + !( + calibAxis->missedStepHistory[0] >= *missedStepsMax && + calibAxis->missedStepHistory[1] >= *missedStepsMax && + calibAxis->missedStepHistory[2] >= *missedStepsMax && + calibAxis->missedStepHistory[3] >= *missedStepsMax && + calibAxis->missedStepHistory[4] >= *missedStepsMax + ) + ) +#else if (((!invertEndStops && !calibAxis->endStopMin()) || (invertEndStops && !calibAxis->endStopMax())) && !movementDone && (*missedSteps < *missedStepsMax)) - +#endif { calibAxis->setStepAxis(); @@ -1624,13 +1681,7 @@ int Movement::calibrateAxis(int axis) //checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]); - //delayMicroseconds(stepDelay); - lastTick = calibrationTicks; - nextTick = calibrationTicks + tickDelay; - while (calibrationTicks < nextTick && calibrationTicks >= lastTick) - { - delayMicroseconds(250); - } + delayMicroseconds(stepDelay); if (stepsCount % (speedHome[axis] * 3) == 0) { @@ -1646,13 +1697,7 @@ int Movement::calibrateAxis(int axis) } calibAxis->resetMotorStep(); - //delayMicroseconds(stepDelay); - lastTick = calibrationTicks; - nextTick = calibrationTicks + tickDelay; - while (calibrationTicks < nextTick && calibrationTicks >= lastTick) - { - delayMicroseconds(250); - } + delayMicroseconds(stepDelay); } else diff --git a/src/MovementAxis.cpp b/src/MovementAxis.cpp index ef14202..2cf2654 100644 --- a/src/MovementAxis.cpp +++ b/src/MovementAxis.cpp @@ -163,6 +163,24 @@ bool MovementAxis::isDriverError() uint8_t MovementAxis::getStatus() { + /**/ + // Nog test + uint32_t missed_step = 0; + TMC2130A->read_REG(FB_TMC_REG_LOST_STEPS, &missed_step); + + if (missed_step > 0) + { + Serial.print("R99"); + Serial.print(" "); + Serial.print(" MIS STPR"); + Serial.print(" "); + Serial.print(missed_step); + Serial.print("\r\n "); + } + + + + // Read status from driver uint32_t gstat = 0; TMC2130A->read_REG(FB_TMC_REG_GSTAT, &gstat); diff --git a/src/TMC2130.cpp b/src/TMC2130.cpp index 730ff12..6def8b5 100644 --- a/src/TMC2130.cpp +++ b/src/TMC2130.cpp @@ -114,6 +114,7 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current, tb->init(); + // Set the micro steps switch (microsteps) { case 1: data = 8; @@ -143,6 +144,8 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current, tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(data) << FB_TMC_CHOPCONF_MRES, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_MRES] << FB_TMC_CHOPCONF_MRES); + // Set the current + uint16_t mA = current; float multiplier = 0.5; float RS = 0.11; @@ -155,18 +158,59 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current, data |= ((uint32_t(16)&FB_TMC_IHOLDDELAY_MASK) << FB_TMC_IHOLDDELAY); tb->write_REG(FB_TMC_REG_IHOLD_IRUN, data); - tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(sensitivity) << FB_TMC_COOLCONF_SGT, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SGT] << FB_TMC_COOLCONF_SGT); + + + /* + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(sensitivity) << FB_TMC_COOLCONF_SGT, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SGT] << FB_TMC_COOLCONF_SGT); tb->set_GCONF(FB_TMC_GCONF_I_SCALE_ANALOG, 1); tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(3) << FB_TMC_CHOPCONF_TOFF, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_TOFF] << FB_TMC_CHOPCONF_TOFF); tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(1) << FB_TMC_CHOPCONF_TBL, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_TBL] << FB_TMC_CHOPCONF_TBL); - tb->set_GCONF(FB_TMC_GCONF_DIAG1_STALL, 1); - tb->set_GCONF(FB_TMC_GCONF_DIAG1_ONSTATE, 1); + tb->set_GCONF(FB_TMC_GCONF_DIAG1_STALL, 1); // even afgezet voor test + tb->set_GCONF(FB_TMC_GCONF_DIAG1_ONSTATE, 1); // even afgezet voor test tb->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK); tb->write_REG(FB_TMC_REG_THIGH, 0xFFFFF & FB_TMC_THIGH_MASK); tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(5) << FB_TMC_COOLCONF_SEMIN, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SEMIN] << FB_TMC_COOLCONF_SEMIN); tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(15) << FB_TMC_COOLCONF_SEMAX, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SEMAX] << FB_TMC_COOLCONF_SEMAX); +*/ + + // om later te testen in dcStep modus + + // DirectX for test + //tb->alter_REG(FB_TMC_REG_XDIRECT, uint32_t(248) << FB_TMC_XDIRECT_COIL_A, FB_TMC_XDIRECT_COIL_A_MASK << FB_TMC_XDIRECT_COIL_A); + //tb->alter_REG(FB_TMC_REG_XDIRECT, uint32_t(248) << FB_TMC_XDIRECT_COIL_B, FB_TMC_XDIRECT_COIL_B_MASK << FB_TMC_XDIRECT_COIL_B); + + // set to dcStep mode + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(1) << FB_TMC_CHOPCONF_VHIGHFS, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_VHIGHFS] << FB_TMC_CHOPCONF_VHIGHFS); + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(1) << FB_TMC_CHOPCONF_VHIGHCHM, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_VHIGHCHM] << FB_TMC_CHOPCONF_VHIGHCHM); + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(8) << FB_TMC_CHOPCONF_TOFF, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_TOFF] << FB_TMC_CHOPCONF_TOFF); + + // Set minimum speed + tb->write_REG(FB_TMC_REG_VDCMIN, uint32_t(0) & FB_TMC_VDCMIN_MASK); + tb->write_REG(FB_TMC_REG_TCOOLTHRS, uint32_t(0) & FB_TMC_TCOOLTHRS_MASK); + + + // Set maximum speed + tb->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK); + + // Set sensitivity + tb->alter_REG(FB_TMC_REG_DCCTRL, uint32_t(1024) << FB_TMC_DCCTRL_DC_TIME, FB_TMC_DCCTRL_DC_TIME_MASK); + tb->alter_REG(FB_TMC_REG_DCCTRL, uint32_t(256) << FB_TMC_DCCTRL_DC_SG, FB_TMC_DCCTRL_DC_SG_MASK); + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(sensitivity) << FB_TMC_COOLCONF_SGT, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SGT] << FB_TMC_COOLCONF_SGT); + + // Set chopper + tb->alter_REG(FB_TMC_REG_CHOPCONF, uint32_t(8) << FB_TMC_CHOPCONF_TOFF, FB_TMC_CHOPCONF_MASKS[FB_TMC_CHOPCONF_TOFF] << FB_TMC_CHOPCONF_TOFF); + + // enable diagnostics + tb->set_GCONF(FB_TMC_GCONF_DIAG0_STALL, 1); + tb->set_GCONF(FB_TMC_GCONF_DIAG0_ERROR, 1); + + tb->set_GCONF(FB_TMC_GCONF_DIAG1_STALL, 0); + tb->set_GCONF(FB_TMC_GCONF_DIAG1_INDEX, 0); + tb->set_GCONF(FB_TMC_GCONF_DIAG1_ONSTATE, 0); + + tb->set_GCONF(FB_TMC_GCONF_DIAG1_STEPS_SKIPPED, 0); /* // 2020-05-15 copy of settings before experiments tb->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK);