Calibration changes
parent
8f242c0bd5
commit
d7a4220a2d
103
src/Movement.cpp
103
src/Movement.cpp
|
@ -1233,10 +1233,34 @@ int Movement::calibrateAxis(int axis)
|
||||||
unsigned long nextTick = 0;
|
unsigned long nextTick = 0;
|
||||||
unsigned long lastTick = 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
|
// Prepare for movement
|
||||||
tickDelay = (1000.0 * 1000.0 / MOVEMENT_INTERRUPT_SPEED / speedHome[axis] / 2);
|
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;
|
stepDelay = 100000 / speedHome[axis] / 2;
|
||||||
|
#endif
|
||||||
|
|
||||||
Serial.print("R99");
|
Serial.print("R99");
|
||||||
Serial.print(" ");
|
Serial.print(" ");
|
||||||
|
@ -1420,17 +1444,43 @@ int Movement::calibrateAxis(int axis)
|
||||||
/**/
|
/**/
|
||||||
//if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax))
|
//if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax))
|
||||||
//if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !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))
|
if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
|
||||||
|
#endif
|
||||||
{
|
{
|
||||||
|
|
||||||
calibAxis->setStepAxis();
|
calibAxis->setStepAxis();
|
||||||
|
delayMicroseconds(stepDelay);
|
||||||
lastTick = calibrationTicks;
|
|
||||||
nextTick = calibrationTicks + tickDelay;
|
|
||||||
while (calibrationTicks < nextTick && calibrationTicks >= lastTick)
|
|
||||||
{
|
|
||||||
delayMicroseconds(250);
|
|
||||||
}
|
|
||||||
|
|
||||||
stepsCount++;
|
stepsCount++;
|
||||||
if (stepsCount % (speedHome[axis] * 3) == 0)
|
if (stepsCount % (speedHome[axis] * 3) == 0)
|
||||||
|
@ -1461,13 +1511,7 @@ int Movement::calibrateAxis(int axis)
|
||||||
|
|
||||||
calibAxis->resetMotorStep();
|
calibAxis->resetMotorStep();
|
||||||
|
|
||||||
//delayMicroseconds(stepDelay);
|
delayMicroseconds(stepDelay);
|
||||||
lastTick = calibrationTicks;
|
|
||||||
nextTick = calibrationTicks + tickDelay;
|
|
||||||
while (calibrationTicks < nextTick && calibrationTicks >= lastTick)
|
|
||||||
{
|
|
||||||
delayMicroseconds(250);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -1615,8 +1659,21 @@ int Movement::calibrateAxis(int axis)
|
||||||
/**/
|
/**/
|
||||||
//if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
|
//if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
|
||||||
//if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !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))
|
if (((!invertEndStops && !calibAxis->endStopMin()) || (invertEndStops && !calibAxis->endStopMax())) && !movementDone && (*missedSteps < *missedStepsMax))
|
||||||
|
#endif
|
||||||
{
|
{
|
||||||
|
|
||||||
calibAxis->setStepAxis();
|
calibAxis->setStepAxis();
|
||||||
|
@ -1624,13 +1681,7 @@ int Movement::calibrateAxis(int axis)
|
||||||
|
|
||||||
//checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
|
//checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
|
||||||
|
|
||||||
//delayMicroseconds(stepDelay);
|
delayMicroseconds(stepDelay);
|
||||||
lastTick = calibrationTicks;
|
|
||||||
nextTick = calibrationTicks + tickDelay;
|
|
||||||
while (calibrationTicks < nextTick && calibrationTicks >= lastTick)
|
|
||||||
{
|
|
||||||
delayMicroseconds(250);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (stepsCount % (speedHome[axis] * 3) == 0)
|
if (stepsCount % (speedHome[axis] * 3) == 0)
|
||||||
{
|
{
|
||||||
|
@ -1646,13 +1697,7 @@ int Movement::calibrateAxis(int axis)
|
||||||
}
|
}
|
||||||
|
|
||||||
calibAxis->resetMotorStep();
|
calibAxis->resetMotorStep();
|
||||||
//delayMicroseconds(stepDelay);
|
delayMicroseconds(stepDelay);
|
||||||
lastTick = calibrationTicks;
|
|
||||||
nextTick = calibrationTicks + tickDelay;
|
|
||||||
while (calibrationTicks < nextTick && calibrationTicks >= lastTick)
|
|
||||||
{
|
|
||||||
delayMicroseconds(250);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
|
|
|
@ -163,6 +163,24 @@ bool MovementAxis::isDriverError()
|
||||||
|
|
||||||
uint8_t MovementAxis::getStatus() {
|
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;
|
uint32_t gstat = 0;
|
||||||
TMC2130A->read_REG(FB_TMC_REG_GSTAT, &gstat);
|
TMC2130A->read_REG(FB_TMC_REG_GSTAT, &gstat);
|
||||||
|
|
||||||
|
|
|
@ -114,6 +114,7 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current,
|
||||||
|
|
||||||
tb->init();
|
tb->init();
|
||||||
|
|
||||||
|
// Set the micro steps
|
||||||
switch (microsteps) {
|
switch (microsteps) {
|
||||||
case 1:
|
case 1:
|
||||||
data = 8;
|
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);
|
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;
|
uint16_t mA = current;
|
||||||
float multiplier = 0.5;
|
float multiplier = 0.5;
|
||||||
float RS = 0.11;
|
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);
|
data |= ((uint32_t(16)&FB_TMC_IHOLDDELAY_MASK) << FB_TMC_IHOLDDELAY);
|
||||||
tb->write_REG(FB_TMC_REG_IHOLD_IRUN, data);
|
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->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(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->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_STALL, 1); // even afgezet voor test
|
||||||
tb->set_GCONF(FB_TMC_GCONF_DIAG1_ONSTATE, 1);
|
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_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK);
|
||||||
tb->write_REG(FB_TMC_REG_THIGH, 0xFFFFF & FB_TMC_THIGH_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(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);
|
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
|
/* // 2020-05-15 copy of settings before experiments
|
||||||
tb->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK);
|
tb->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK);
|
||||||
|
|
Loading…
Reference in New Issue