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

Merge pull request #127 from TimEvWw/master 

Encoder update for TMC
pull/128/head
Tim Evers 2020-02-06 22:01:11 +01:00 committed by GitHub
parent e71ec1c856 7dc7f346fd
commit f179a213ca
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 329 additions and 253 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

221
src/Movement.cpp
View File

@ -241,85 +241,107 @@ void Movement::loadSettings()
motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z); motorCurrentZ = ParameterList::getInstance()->getValue(MOVEMENT_MOTOR_CURRENT_Z);
stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z); stallSensitivityZ = ParameterList::getInstance()->getValue(MOVEMENT_STALL_SENSITIVITY_Z);
microStepsX = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z); microStepsZ = ParameterList::getInstance()->getValue(MOVEMENT_MICROSTEPS_Z);
stallSensitivityX = 0; /**/
stallSensitivityY = 0;
stallSensitivityZ = 0; //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 (microStepsX <= 0) { microStepsX = 1; }
if (microStepsY <= 0) { microStepsY = 1; } if (microStepsY <= 0) { microStepsY = 1; }
if (microStepsZ <= 0) { microStepsZ = 1; } if (microStepsZ <= 0) { microStepsZ = 1; }
axisX.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); axisX.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX);
axisY.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); axisY.loadSettingsTMC2130(motorCurrentY, stallSensitivityY, microStepsY);
axisZ.loadSettingsTMC2130(motorCurrentX, stallSensitivityX, microStepsX); axisZ.loadSettingsTMC2130(motorCurrentZ, stallSensitivityZ, microStepsZ);
} }
#endif #endif
static int missedCount = 0;
void Movement::test() void Movement::test()
{ {
axisX.enableMotor();
//axisY.test();
//axisX.enableMotor(); //axisX.enableMotor();
//axisX.setMotorStep(); //axisX.setMotorStep();
//delayMicroseconds(10); //delayMicroseconds(500);
//axisX.setMotorStep(); //TMC2130X.read_STAT();
//delayMicroseconds(10); //delayMicroseconds(500);
//if (axisX.stallDetected()) { testA++; }
//testB++;
bool stallGuard = false;
bool standStill = false;
uint8_t status_x = 0;
//digitalWrite(X_ENABLE_PIN, LOW);
axisX.enableMotor();
//digitalWrite(X_STEP_PIN, HIGH); //digitalWrite(X_STEP_PIN, HIGH);
//delayMicroseconds(10); axisX.setMotorStep();
delayMicroseconds(500);
//digitalWrite(X_STEP_PIN, LOW); //digitalWrite(X_STEP_PIN, LOW);
//delayMicroseconds(10); axisX.resetMotorStep();
//axisX.setMotorStepWriteTMC2130(); TMC2130X.read_STAT();
//axisX.test();
//Serial.print("R99"); status_x = TMC2130X.getStatus();
//Serial.print(" mot x = "); stallGuard = status_x & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
//Serial.print(axisX.currentPosition()); standStill = status_x & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
//Serial.print(" enc x = "); if (stallGuard || standStill) {
//Serial.print(encoderX.currentPosition()); testA++;
//Serial.print("\r\n"); }
//Serial.print("R99"); //if (axisX.stallDetected()) {
//Serial.print(" mot y = "); // testA++;
//Serial.print(axisY.currentPosition()); //}
//Serial.print(" enc y = ");
//Serial.print(encoderY.currentPosition());
//Serial.print("\r\n"); testB++;
delayMicroseconds(500);
//Serial.print("R99");
//Serial.print(" mot z = ");
//Serial.print(axisZ.currentPosition());
//Serial.print(" enc z = ");
//Serial.print(encoderZ.currentPosition());
//Serial.print("\r\n");
// read changes in encoder
//encoderX.readEncoder();
//encoderY.readEncoder();
//encoderZ.readEncoder();
//reportPosition();
//bool test = axisX.endStopMin();
//Serial.print("R99");
//Serial.print(" test = ");
//Serial.print(test);
//Serial.print("\r\n");
} }
void Movement::test2() void Movement::test2()
{ {
Serial.print("*");
axisX.setMotorStep(); Serial.print(testA);
//CurrentState::getInstance()->printPosition(); Serial.print("/");
//encoderX.test(); Serial.print(testB);
//encoderY.test(); Serial.println();
//encoderZ.test();
//testA = 0;
} }
/** /**
@ -503,25 +525,22 @@ int Movement::moveToCoords(double xDestScaled, double yDestScaled, double zDestS
#endif #endif
/**/ /**/
if (loopCounts % 1000 == 0) //if (loopCounts % 1000 == 0)
{ //{
// Serial.print("R99");
// Serial.print(" missed step ");
// Serial.print(motorConsMissedSteps[1]);
// Serial.print(" axis pos ");
// Serial.print(axisY.currentPosition());
// Serial.print("\r\n");
//Serial.print("R99"); // Serial.print("X - ");
//Serial.print(" missed step "); // axisX.test();
//Serial.print(motorConsMissedSteps[1]);
//Serial.print(" axis pos ");
//Serial.print(axisY.currentPosition());
//Serial.print("\r\n");
//Serial.print("X - "); // Serial.print("Y - ");
//axisX.test(); // axisY.test();
//}
//Serial.print("Y - "); //loopCounts++;
//axisY.test();
}
loopCounts++;
checkAxisSubStatus(&axisX, &axisSubStep[0]); checkAxisSubStatus(&axisX, &axisSubStep[0]);
checkAxisSubStatus(&axisY, &axisSubStep[1]); checkAxisSubStatus(&axisY, &axisSubStep[1]);
@ -1391,22 +1410,56 @@ void Movement::checkAxisVsEncoder(MovementAxis *axis, MovementEncoder *encoder,
// Serial.print(*lastPosition); // Serial.print(*lastPosition);
// */ // */
// //
// if (*encoderEnabled) {
// if (axis->stallDetected()) { //if (axis->stallDetected()) {
// // In case of stall detection, count this as a missed step // Serial.print("X");
// (*missedSteps)++; //}
// axis->setCurrentPosition(*lastPosition); //else
// } //{
// else { // Serial.print(".");
// // Decrease amount of missed steps if there are no missed step //}
// if (*missedSteps > 0)
// { /**/
// (*missedSteps) -= (*encoderStepDecay); bool stallGuard = false;
// } bool standStill = false;
// *lastPosition = axis->currentPosition(); uint8_t status = 0;
// encoder->setPosition(axis->currentPosition());
// } if (*encoderEnabled) {
// }
//TMC2130X.read_STAT();
status = axis->getStatus();
stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
if (stallGuard || standStill) {
//Serial.print(".");
// In case of stall detection, count this as a missed step
(*missedSteps)++;
axis->setCurrentPosition(*lastPosition);
//if (int(*missedSteps) % 10 == 0) {
// Serial.println();
// Serial.print(*missedSteps);
// Serial.print("/");
// Serial.print(axis->currentPosition());
// Serial.println();
//}
}
else {
// Decrease amount of missed steps if there are no missed step
if (*missedSteps > 0)
{
(*missedSteps) -= (*encoderStepDecay);
}
*lastPosition = axis->currentPosition();
encoder->setPosition(axis->currentPosition());
}
}
// //
// //Serial.print(" new last pos "); // //Serial.print(" new last pos ");
// //Serial.print(*lastPosition); // //Serial.print(*lastPosition);

5
src/Movement.h
View File

@ -145,6 +145,11 @@ private:
int axisServicedNext = 0; int axisServicedNext = 0;
bool motorMotorsEnabled = false; bool motorMotorsEnabled = false;
int testA = 0;
int testB = 0;
}; };
#endif /* MOVEMENT_H_ */ #endif /* MOVEMENT_H_ */

27
src/MovementAxis.cpp
View File

@ -97,10 +97,18 @@ void MovementAxis::initTMC2130()
TMC2130A = &TMC2130Z; TMC2130A = &TMC2130Z;
} }
setMotorStepWrite = &MovementAxis::setMotorStepWriteTMC2130; setMotorStepWrite = &MovementAxis::setMotorStepWriteDefault;
setMotorStepWrite2 = &MovementAxis::setMotorStepWriteTMC2130_2; setMotorStepWrite2 = &MovementAxis::setMotorStepWriteDefault2;
resetMotorStepWrite = &MovementAxis::resetMotorStepWriteTMC2130; resetMotorStepWrite = &MovementAxis::resetMotorStepWriteDefault;
resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteTMC2130_2; resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteDefault2;
/**/ // Not using the edge driving now, only standard pulse driving
// as edge driving causes issues with stall detection routines
//setMotorStepWrite = &MovementAxis::setMotorStepWriteTMC2130;
//setMotorStepWrite2 = &MovementAxis::setMotorStepWriteTMC2130_2;
//resetMotorStepWrite = &MovementAxis::resetMotorStepWriteTMC2130;
//resetMotorStepWrite2 = &MovementAxis::resetMotorStepWriteTMC2130_2;
TMC2130A->init(); TMC2130A->init();
@ -120,14 +128,9 @@ void MovementAxis::loadSettingsTMC2130(int motorCurrent, int stallSensitivity,
} }
} }
bool MovementAxis::stallDetected() { uint8_t MovementAxis::getStatus() {
/**/ return TMC2130X.read_STAT();
bool stallGuard = TMC2130A->getStatus() & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false; //return TMC2130A->getStatus();
bool standStill = TMC2130A->getStatus() & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
//return (TMC2130A->isStandstill() || TMC2130A->isStallguard());
//return stallGuard || standStill;
return false;
} }
uint16_t MovementAxis::getLoad() { uint16_t MovementAxis::getLoad() {

4
src/MovementAxis.h
View File

@ -95,8 +95,8 @@ public:
#if defined(FARMDUINO_EXP_V20) #if defined(FARMDUINO_EXP_V20)
void initTMC2130(); void initTMC2130();
void loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps); void loadSettingsTMC2130(int motorCurrent, int stallSensitivity, int microSteps);
bool stallDetected();
uint16_t getLoad(); uint16_t getLoad();
#endif #endif
#if defined(FARMDUINO_EXP_V20) #if defined(FARMDUINO_EXP_V20)
@ -105,6 +105,8 @@ public:
void resetMotorStepWriteTMC2130(); void resetMotorStepWriteTMC2130();
void resetMotorStepWriteTMC2130_2(); void resetMotorStepWriteTMC2130_2();
uint8_t getStatus();
unsigned int getLostSteps(); unsigned int getLostSteps();
bool tmcStep = true; bool tmcStep = true;

65
src/TMC2130.cpp
View File

@ -23,7 +23,7 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current,
//Serial.print(sensitivity); //Serial.print(sensitivity);
//Serial.println(" "); //Serial.println(" ");
/**/ /*
tb->init(); tb->init();
uint8_t data = 0; uint8_t data = 0;
@ -107,6 +107,69 @@ void loadTMC2130ParametersMotor(TMC2130_Basics *tb, int microsteps, int current,
tb->set_CHOPCONF(FB_TMC_COOLCONF_SEMAX, 0b01); tb->set_CHOPCONF(FB_TMC_COOLCONF_SEMAX, 0b01);
tb->set_CHOPCONF(FB_TMC_COOLCONF_SGT, sensitivity); tb->set_CHOPCONF(FB_TMC_COOLCONF_SGT, sensitivity);
*/
uint32_t data = 0;
uint32_t value = 0;
tb->init();
switch (microsteps) {
case 1:
data = 8;
break;
case 2:
data = 7;
break;
case 4:
data = 6;
break;
case 8:
data = 5;
break;
case 16:
data = 4;
break;
case 32:
data = 3;
break;
case 64:
data = 2;
break;
case 128:
data = 1;
break;
}
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);
uint16_t mA = current;
float multiplier = 0.5;
float RS = 0.11;
uint8_t CS = 32.0*1.41421*mA / 1000.0*(RS + 0.02) / 0.325 - 1;
if (CS < 16) {
CS = 32.0*1.41421*mA / 1000.0*(RS + 0.02) / 0.180 - 1;
}
data = ((uint32_t(CS)&FB_TMC_IHOLD_MASK) << FB_TMC_IHOLD);
data |= ((uint32_t(CS)&FB_TMC_IRUN_MASK) << FB_TMC_IRUN);
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->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->write_REG(FB_TMC_REG_TCOOLTHRS, 0xFFFFF & FB_TMC_TCOOLTHRS_MASK);
tb->write_REG(FB_TMC_REG_THIGH, 0 & 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(2) << 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(0b01) << FB_TMC_COOLCONF_SEDN, FB_TMC_CHOPCONF_MASKS[FB_TMC_COOLCONF_SEDN] << FB_TMC_COOLCONF_SEDN);
delay(100);
} }
#endif #endif

22
src/TMC2130_Basics.cpp
View File

@ -19,14 +19,15 @@ void TMC2130_Basics::init() {
void TMC2130_Basics::init_SPI() { void TMC2130_Basics::init_SPI() {
SPI.setDataMode(FB_TMC_SPI_DATA_MODE); SPI.setDataMode(FB_TMC_SPI_DATA_MODE);
SPI.setBitOrder(FB_TMC_SPI_BIT_ORDER); SPI.setBitOrder(FB_TMC_SPI_BIT_ORDER);
SPI.setClockDivider(FB_TMC_SPI_CLOCK_DIVIDER); //SPI.setClockDivider(FB_TMC_SPI_CLOCK_DIVIDER);/**
SPI.setClockDivider(SPI_CLOCK_DIV4);
SPI.begin(); SPI.begin();
} }
// read status // read status
uint8_t TMC2130_Basics::read_STAT() uint8_t TMC2130_Basics::read_STAT()
{ {
init_SPI(); //init_SPI();
digitalWrite(_csPin, LOW); digitalWrite(_csPin, LOW);
// read address // read address
@ -110,6 +111,20 @@ uint8_t TMC2130_Basics::alter_REG(uint8_t address, uint32_t data, uint32_t mask)
return _status; return _status;
} }
// check the stallguard status
boolean TMC2130_Basics::isStallguard()
{
return _status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
}
// check the standstill status
boolean TMC2130_Basics::isStandstill()
{
return _status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
}
// set single bits in the GCONF register // set single bits in the GCONF register
uint8_t TMC2130_Basics::set_GCONF(uint8_t position, uint8_t value) uint8_t TMC2130_Basics::set_GCONF(uint8_t position, uint8_t value)
{ {
@ -129,4 +144,5 @@ uint8_t TMC2130_Basics::set_CHOPCONF(uint8_t position, uint8_t value)
uint8_t TMC2130_Basics::getStatus() uint8_t TMC2130_Basics::getStatus()
{ {
return _status; return _status;
} }

5
src/TMC2130_Basics.h
View File

@ -29,8 +29,9 @@ public:
//boolean isReset(); //boolean isReset();
//boolean isError(); //boolean isError();
//boolean isStallguard(); boolean isStallguard();
//boolean isStandstill(); boolean isStandstill();
//String debug(); //String debug();
private: private:

133
src/farmbot_arduino_controller.cpp
View File

@ -1,9 +1,10 @@
// Do not remove the include below // Do not remove the include below
#include "farmbot_arduino_controller.h" #include "farmbot_arduino_controller.h"
#if !defined(FARMDUINO_EXP_V20) /**/
//#if !defined(FARMDUINO_EXP_V20)
#include "TimerOne.h" #include "TimerOne.h"
#endif //#endif
bool stepperInit = false; bool stepperInit = false;
bool stepperFlip = false; bool stepperFlip = false;
@ -51,7 +52,7 @@ unsigned long interruptDurationMax = 0;
bool interruptBusy = false; bool interruptBusy = false;
int interruptSecondTimer = 0; int interruptSecondTimer = 0;
#if !defined(FARMDUINO_EXP_V20) //#if !defined(FARMDUINO_EXP_V20)
void interrupt(void) void interrupt(void)
{ {
if (!debugInterrupt) if (!debugInterrupt)
@ -68,11 +69,11 @@ void interrupt(void)
} }
} }
} }
#endif //#endif
/**/ // unsigned long intrCounter = 0; /**/ // unsigned long intrCounter = 0;
#if defined(FARMDUINO_EXP_V20) #if defined(FARMDUINO_EXP_V20xxx)
ISR(TIMER2_OVF_vect) { ISR(TIMER2_OVF_vect) {
if (interruptBusy == false) if (interruptBusy == false)
@ -121,6 +122,7 @@ void periodicChecksAndReport()
if ((currentTime - lastAction) > reportingPeriod) if ((currentTime - lastAction) > reportingPeriod)
{ {
// After an idle time, send the idle message // After an idle time, send the idle message
//Movement::getInstance()->test2();
if (CurrentState::getInstance()->isEmergencyStop()) if (CurrentState::getInstance()->isEmergencyStop())
{ {
@ -281,11 +283,10 @@ void checkParamsChanged()
CurrentState::getInstance()->printQAndNewLine(); CurrentState::getInstance()->printQAndNewLine();
} }
/*
#if defined(FARMDUINO_EXP_V20) #if defined(FARMDUINO_EXP_V20)
loadTMC2130paraeters(); Movement::getInstance()->loadSettingsTMC2130();
#endif #endif
*/
Movement::getInstance()->loadSettings(); Movement::getInstance()->loadSettings();
PinGuard::getInstance()->loadConfig(); PinGuard::getInstance()->loadConfig();
@ -584,18 +585,18 @@ void startInterrupt()
// Interrupt management code library written by Paul Stoffregen // Interrupt management code library written by Paul Stoffregen
// The default time 100 micro seconds // The default time 100 micro seconds
#if !defined(FARMDUINO_EXP_V20) //#if !defined(FARMDUINO_EXP_V20)
Timer1.attachInterrupt(interrupt); Timer1.attachInterrupt(interrupt);
Timer1.initialize(MOVEMENT_INTERRUPT_SPEED); Timer1.initialize(MOVEMENT_INTERRUPT_SPEED);
Timer1.start(); Timer1.start();
#endif //#endif
#if defined(FARMDUINO_EXP_V20) //#if defined(FARMDUINO_EXP_V20)
Serial.println("set timer"); // Serial.println("set timer");
TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow // TIMSK2 = (TIMSK2 & B11111110) | 0x01; // Enable timer overflow
TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1 // TCCR2B = (TCCR2B & B11111000) | 0x01; // Set divider to 1
OCR2A = 4; // Set overflow to 4 for total of 64 µs // OCR2A = 4; // Set overflow to 4 for total of 64 ľs
#endif //#endif
} }
void homeOnBoot() void homeOnBoot()
@ -802,15 +803,28 @@ void setupTestForDebug()
//loadTMC2130ParametersMotor(&controllerTMC2130_Z, 8, 500, 0); delay(100); //loadTMC2130ParametersMotor(&controllerTMC2130_Z, 8, 500, 0); delay(100);
//loadTMC2130ParametersMotor(&controllerTMC2130_E, 8, 500, 0); delay(100); //loadTMC2130ParametersMotor(&controllerTMC2130_E, 8, 500, 0); delay(100);
/*
Serial.println("Init");
loadTMC2130ParametersMotor(&TMC2130X, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130Y, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130Z, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130E, 8, 200, 0);
*/
} }
bool left = false; bool left = false;
int missedX = 0;
int missedY = 0;
int missedZ = 0;
int missedE = 0;
void runTestForDebug() void runTestForDebug()
{ {
/*
currentTime = millis(); currentTime = millis();
if (currentTime < lastAction) if (currentTime < lastAction)
{ {
@ -824,27 +838,40 @@ void runTestForDebug()
if ((currentTime - lastAction) > reportingPeriod) if ((currentTime - lastAction) > reportingPeriod)
{ {
blinkLed(); blinkLed();
Serial.print("."); //Serial.print(".");
lastAction = currentTime; lastAction = currentTime;
Serial.print(">");
Serial.print(" X = ");
Serial.print(missedX);
Serial.print(" Y = ");
Serial.print(missedY);
Serial.print(" Z = ");
Serial.print(missedZ);
Serial.print(" E = ");
Serial.print(missedE);
Serial.print(CRLF);
if (left) { if (left) {
left = false; left = false;
digitalWrite(X_DIR_PIN, LOW); //digitalWrite(X_DIR_PIN, LOW);
digitalWrite(Y_DIR_PIN, LOW); //digitalWrite(Y_DIR_PIN, LOW);
digitalWrite(Z_DIR_PIN, LOW); //digitalWrite(Z_DIR_PIN, LOW);
digitalWrite(E_DIR_PIN, LOW); //digitalWrite(E_DIR_PIN, LOW);
} }
else { else {
left = true; left = true;
digitalWrite(X_DIR_PIN, HIGH); //digitalWrite(X_DIR_PIN, HIGH);
digitalWrite(Y_DIR_PIN, HIGH); //digitalWrite(Y_DIR_PIN, HIGH);
digitalWrite(Z_DIR_PIN, HIGH); //digitalWrite(Z_DIR_PIN, HIGH);
digitalWrite(E_DIR_PIN, HIGH); //digitalWrite(E_DIR_PIN, HIGH);
} }
} }
} }
*/
/*
// make a step // make a step
digitalWrite(X_STEP_PIN, HIGH); digitalWrite(X_STEP_PIN, HIGH);
digitalWrite(Y_STEP_PIN, HIGH); digitalWrite(Y_STEP_PIN, HIGH);
@ -858,38 +885,44 @@ void runTestForDebug()
digitalWrite(E_STEP_PIN, LOW); digitalWrite(E_STEP_PIN, LOW);
delayMicroseconds(100); delayMicroseconds(100);
/* bool stallGuard = false;
bool standStill = false;
uint8_t status = 0;
*/
Movement::getInstance()->test();
/*
TMC2130X.read_STAT(); TMC2130X.read_STAT();
if (TMC2130X.isStandstill() || TMC2130X.isStallguard()) {
Serial.print("X");
}
TMC2130Y.read_STAT(); TMC2130Y.read_STAT();
if (TMC2130Y.isStandstill() || TMC2130Y.isStallguard()) {
Serial.print("Y");
}
TMC2130Z.read_STAT(); TMC2130Z.read_STAT();
if (TMC2130Z.isStandstill() || TMC2130Z.isStallguard()) {
Serial.print("Z");
}
TMC2130E.read_STAT(); TMC2130E.read_STAT();
if (TMC2130E.isStandstill() || TMC2130E.isStallguard()) {
Serial.print("E");
}
*/
status = TMC2130X.getStatus();
stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
if (stallGuard || standStill) { missedX++;}
/* status = TMC2130Y.getStatus();
digitalWrite(X_STEP_PIN, HIGH); stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
digitalWrite(E_STEP_PIN, HIGH); standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
delayMicroseconds(100); if (stallGuard || standStill) { missedY++; }
digitalWrite(X_STEP_PIN, LOW); status = TMC2130Z.getStatus();
digitalWrite(E_STEP_PIN, LOW); stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
delayMicroseconds(100); standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
*/ if (stallGuard || standStill) { missedZ++; }
status = TMC2130E.getStatus();
stallGuard = status & FB_TMC_SPISTATUS_STALLGUARD_MASK ? true : false;
standStill = status & FB_TMC_SPISTATUS_STANDSTILL_MASK ? true : false;
if (stallGuard || standStill) { missedE++; }
*/
//if (TMC2130X.isStandstill() || TMC2130X.isStallguard()) {missedX++;}
//if (TMC2130Y.isStandstill() || TMC2130Y.isStallguard()) {missedY++;}
//if (TMC2130Z.isStandstill() || TMC2130Z.isStallguard()) {missedZ++;}
//if (TMC2130E.isStandstill() || TMC2130E.isStallguard()) {missedE++;}
//Movement::getInstance()->test(); //Movement::getInstance()->test();
//delayMicroseconds(100); //delayMicroseconds(100);

100
src/src.ino
View File

@ -43,106 +43,6 @@ void setup()
Serial.print(SPACE); Serial.print(SPACE);
Serial.print("ARDUINO STARTUP COMPLETE"); Serial.print("ARDUINO STARTUP COMPLETE");
Serial.print(CRLF); Serial.print(CRLF);
/*
Serial.begin(115200); //init serial port and set baudrate
while (!Serial); //wait for serial port to connect (needed for Leonardo only)
Serial.println("Start...");
*/
/*
// pins
pinMode(X_ENABLE_PIN, OUTPUT);
pinMode(X_DIR_PIN, OUTPUT);
pinMode(X_STEP_PIN, OUTPUT);
digitalWrite(X_ENABLE_PIN, HIGH); // disable driver
digitalWrite(X_DIR_PIN, LOW); // chose direction
digitalWrite(X_STEP_PIN, LOW); // no step yet
pinMode(Y_ENABLE_PIN, OUTPUT);
pinMode(Y_DIR_PIN, OUTPUT);
pinMode(Y_STEP_PIN, OUTPUT);
digitalWrite(Y_ENABLE_PIN, HIGH); // disable driver
digitalWrite(Y_DIR_PIN, LOW); // chose direction
digitalWrite(Y_STEP_PIN, LOW); // no step yet
pinMode(Z_ENABLE_PIN, OUTPUT);
pinMode(Z_DIR_PIN, OUTPUT);
pinMode(Z_STEP_PIN, OUTPUT);
digitalWrite(Z_ENABLE_PIN, HIGH); // disable driver
digitalWrite(Z_DIR_PIN, LOW); // chose direction
digitalWrite(Z_STEP_PIN, LOW); // no step yet
pinMode(E_ENABLE_PIN, OUTPUT);
pinMode(E_DIR_PIN, OUTPUT);
pinMode(E_STEP_PIN, OUTPUT);
digitalWrite(E_ENABLE_PIN, HIGH); // disable driver
digitalWrite(E_DIR_PIN, LOW); // chose direction
digitalWrite(E_STEP_PIN, LOW); // no step yet
*/
/*
Serial.println("Init");
loadTMC2130ParametersMotor(&TMC2130X, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130Y, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130Z, 8, 200, 0);
loadTMC2130ParametersMotor(&TMC2130E, 8, 200, 0);
*/
/*
pinMode(X_ENABLE_PIN, OUTPUT);
pinMode(X_DIR_PIN, OUTPUT);
pinMode(X_STEP_PIN, OUTPUT);
digitalWrite(X_ENABLE_PIN, HIGH); // disable driver
digitalWrite(X_DIR_PIN, LOW); // chose direction
digitalWrite(X_STEP_PIN, LOW); // no step yet
pinMode(Y_ENABLE_PIN, OUTPUT);
pinMode(Y_DIR_PIN, OUTPUT);
pinMode(Y_STEP_PIN, OUTPUT);
digitalWrite(Y_ENABLE_PIN, HIGH); // disable driver
digitalWrite(Y_DIR_PIN, LOW); // chose direction
digitalWrite(Y_STEP_PIN, LOW); // no step yet
pinMode(Z_ENABLE_PIN, OUTPUT);
pinMode(Z_DIR_PIN, OUTPUT);
pinMode(Z_STEP_PIN, OUTPUT);
digitalWrite(Z_ENABLE_PIN, HIGH); // disable driver
digitalWrite(Z_DIR_PIN, LOW); // chose direction
digitalWrite(Z_STEP_PIN, LOW); // no step yet
pinMode(E_ENABLE_PIN, OUTPUT);
pinMode(E_DIR_PIN, OUTPUT);
pinMode(E_STEP_PIN, OUTPUT);
digitalWrite(E_ENABLE_PIN, HIGH); // disable driver
digitalWrite(E_DIR_PIN, LOW); // chose direction
digitalWrite(E_STEP_PIN, LOW); // no step yet
*/
//loadTMC2130drivers();
//startupTmc();
//loadTMC2130parameters();
/**/
//Movement::getInstance()->initTMC2130();
//loadTMC2130parameters();
//loadTMC2130ParametersMotor(&TMC2130X, 8, 200, 0);
//loadTMC2130ParametersMotor(&TMC2130Y, 8, 200, 0);
//loadTMC2130ParametersMotor(&TMC2130Z, 8, 200, 0);
//loadTMC2130ParametersMotor(&TMC2130E, 8, 200, 0);
//Serial.println("Enable pins");
//digitalWrite(X_ENABLE_PIN, LOW); // enable driver
//digitalWrite(Y_ENABLE_PIN, LOW); // enable driver
//digitalWrite(Z_ENABLE_PIN, LOW); // enable driver
//digitalWrite(E_ENABLE_PIN, LOW); // enable driver
//Serial.println("Enable");
} }
// the loop function runs over and over again until power down or reset // the loop function runs over and over again until power down or reset