spacecruft
/
farmbot-arduino-firmware
1
0
Fork 0
Code Issues Pull Requests Releases Activity

Merge pull request #149 from TimEvWw/master 

Calibration changes
master
Tim Evers 2020-05-21 22:06:16 +02:00 committed by GitHub
parent 9f16a0b585 d7a4220a2d
commit 0c46dc47be
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 139 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
src/Movement.cpp
View File

@ -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

18
src/MovementAxis.cpp
View File

@ -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);

50
src/TMC2130.cpp
View File

@ -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);