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

Doing this a different way, with more #if define. Sigh.

pull/118/head
Tim Evers 2019-03-20 23:07:54 +01:00
parent 2f13084e65
commit 665de68b7e
10 changed files with 295 additions and 1255 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/StepperControl.cpp
View File

@ -121,10 +121,9 @@ StepperControl::StepperControl()
// Create the axis controllers
/**/
//axisX = StepperControlAxisTMC2130();
//axisX = StepperControlAxisA4988();
axisY = StepperControlAxisA4988();
axisZ = StepperControlAxisA4988();
axisX = StepperControlAxis();
axisY = StepperControlAxis();
axisZ = StepperControlAxis();
axisX.channelLabel = 'X';
axisY.channelLabel = 'Y';
@ -399,8 +398,8 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
// Let the interrupt handle all the movements
while ((axisActive[0] || axisActive[1] || axisActive[2]) && !emergencyStop)
{
/**/ axisX.setMotorStep();
/**/ delayMicroseconds(10);
/**/ //axisX.setMotorStep();
/**/ //delayMicroseconds(10);
#if defined(FARMDUINO_V14)
checkEncoders();

7
src/StepperControl.h
View File

@ -19,9 +19,6 @@
#include <stdlib.h>
#include "Command.h"
#include "StepperControlAxisA4988.h"
#include "StepperControlAxisTMC2130.h"
class StepperControl
{
public:
@ -67,8 +64,7 @@ public:
private:
/**/
StepperControlAxisTMC2130 axisX = StepperControlAxisTMC2130();
//StepperControlAxis axisX;
StepperControlAxis axisX;
StepperControlAxis axisY;
StepperControlAxis axisZ;
@ -142,6 +138,7 @@ private:
int axisServiced = 0;
int axisServicedNext = 0;
bool motorMotorsEnabled = false;
};
#endif /* STEPPERCONTROL_H_ */

283
src/StepperControlAxis.cpp
View File

@ -1,11 +1,14 @@
#include "StepperControlAxis.h"
#include <TMC2130Stepper.h>
#if defined(FARMDUINO_EXP_V20)
TMC2130Stepper TMC2130X = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
TMC2130Stepper TMC2130Y = TMC2130Stepper(Y_ENABLE_PIN, Y_DIR_PIN, Y_STEP_PIN, Y_CHIP_SELECT);
TMC2130Stepper TMC2130Z = TMC2130Stepper(Z_ENABLE_PIN, Z_DIR_PIN, Z_STEP_PIN, Z_CHIP_SELECT);
TMC2130Stepper TMC2130E = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
#endif
StepperControlAxis::StepperControlAxis()
{
init();
}
void StepperControlAxis::init()
{
lastCalcLog = 0;
@ -38,6 +41,20 @@ void StepperControlAxis::init()
stepIsOn = false;
setMotorStepWrite = &StepperControlAxis::setMotorStepWriteDefault;
setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteDefault2;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteDefault;
resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteDefault2;
#if defined(FARMDUINO_EXP_V20)
//// TMC2130 Functions
setMotorStepWrite = &StepperControlAxis::setMotorStepWriteTMC2130;
setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteTMC2130_2;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteTMC2130;
resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteTMC2130_2;
#endif
}
void StepperControlAxis::test()
@ -45,9 +62,43 @@ void StepperControlAxis::test()
Serial.print("R99");
Serial.print(" cur loc = ");
Serial.print(coordCurrentPoint);
//Serial.print(" enc loc = ");
//Serial.print(coordEncoderPoint);
//Serial.print(" cons steps missed = ");
//Serial.print(label);
//Serial.print(consMissedSteps);
Serial.print("\r\n");
}
#if defined(FARMDUINO_EXP_V20)
void StepperControlAxis::initTMC2130A()
{
/**/
if (channelLabel == 'X')
{
TMC2130X.begin(); // Initiate pins and registeries
TMC2130X.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130X.stealthChop(1); // Enable extremely quiet stepping
TMC2130E.begin(); // Initiate pins and registeries
TMC2130E.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130E.stealthChop(1); // Enable extremely quiet stepping
}
if (channelLabel == 'Y')
{
TMC2130X.begin(); // Initiate pins and registeries
TMC2130X.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130X.stealthChop(1); // Enable extremely quiet stepping
}
if (channelLabel == 'Z')
{
TMC2130Z.begin(); // Initiate pins and registeries
TMC2130Z.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130Z.stealthChop(1); // Enable extremely quiet stepping
}
}
#endif
unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec)
{
@ -64,6 +115,20 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
movementCrawling = false;
movementMoving = false;
/*
if (abs(sourcePosition) < abs(destinationPosition))
{
staPos = abs(sourcePosition);
endPos = abs(destinationPosition);
}
else
{
staPos = abs(destinationPosition);
endPos = abs(sourcePosition);
}
*/
// Set the possible negative coordinates to all positive numbers
// so the calculation code still works after the changes
staPos = 0;
@ -80,13 +145,19 @@ unsigned int StepperControlAxis::calculateSpeed(long sourcePosition, long curren
unsigned long halfway = ((endPos - staPos) / 2) + staPos;
//unsigned long halfway = ((destinationPosition - sourcePosition) / 2) + sourcePosition;
// Set the homing speed if the position would be out of bounds
if (
(curPos < staPos || curPos > endPos)
// ||
// Also limit the speed to a crawl when the move would pass the home position
// (sourcePosition > 0 && destinationPosition < 0) || (sourcePosition < 0 && destinationPosition > 0)
// (!motorHomeIsUp && currentPosition <= 0) || (motorHomeIsUp && currentPosition >= 0) ||)
)
{
newSpeed = motorSpeedHome;
//newSpeed = minSpeed;
movementCrawling = true;
movementMoving = true;
}
@ -225,6 +296,15 @@ void StepperControlAxis::checkMovement()
axisSpeed = calculateSpeed(coordSourcePoint, coordCurrentPoint, coordDestinationPoint,
motorSpeedMin, motorSpeedMax, motorStepsAcc);
// // Set the moments when the step is set to true and false
// if (axisSpeed > 0)
// {
// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
// stepOnTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed / 2);
// stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed);
// }
}
else
{
@ -255,13 +335,19 @@ void StepperControlAxis::incrementTick()
void StepperControlAxis::checkTiming()
{
//int i;
// moveTicks++;
if (stepIsOn)
{
if (moveTicks >= stepOffTick)
{
// Negative flank for the steps
resetMotorStep();
setTicks();
//stepOnTick = moveTicks + (500000.0 / motorInterruptSpeed / axisSpeed);
}
}
else
@ -270,8 +356,10 @@ void StepperControlAxis::checkTiming()
{
if (moveTicks >= stepOnTick)
{
// Positive flank for the steps
setStepAxis();
//stepOffTick = moveTicks + (1000000.0 / motorInterruptSpeed / axisSpeed);
}
}
}
@ -287,7 +375,6 @@ void StepperControlAxis::setTicks()
void StepperControlAxis::setStepAxis()
{
/**/Serial.print("#");
stepIsOn = true;
@ -376,6 +463,32 @@ void StepperControlAxis::loadMotorSettings(
motorStopAtHome = stopAtHome;
motorMaxSize = maxSize;
motorStopAtMax = stopAtMax;
if (pinStep == 54)
{
setMotorStepWrite = &StepperControlAxis::setMotorStepWrite54;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite54;
}
if (pinStep == 60)
{
setMotorStepWrite = &StepperControlAxis::setMotorStepWrite60;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite60;
}
if (pinStep == 46)
{
setMotorStepWrite = &StepperControlAxis::setMotorStepWrite46;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWrite46;
}
if (pin2Step == 26)
{
setMotorStepWrite2 = &StepperControlAxis::setMotorStepWrite26;
resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWrite26;
}
}
bool StepperControlAxis::loadCoordinates(long sourcePoint, long destinationPoint, bool home)
@ -453,6 +566,7 @@ void StepperControlAxis::disableMotor()
void StepperControlAxis::setDirectionUp()
{
#if !defined(FARMDUINO_EXP_V20)
if (motorMotorInv)
{
digitalWrite(pinDirection, LOW);
@ -470,6 +584,57 @@ void StepperControlAxis::setDirectionUp()
{
digitalWrite(pin2Direction, HIGH);
}
#endif
#if defined(FARMDUINO_EXP_V20)
if (channelLabel == 'X')
{
if (motorMotorInv)
{
TMC2130X.shaft_dir(0);
}
else
{
TMC2130X.shaft_dir(1);
}
if (motorMotor2Enl && motorMotor2Inv)
{
TMC2130E.shaft_dir(0);
}
else
{
TMC2130E.shaft_dir(1);
}
}
if (channelLabel == 'Y')
{
if (motorMotorInv)
{
TMC2130Y.shaft_dir(0);
}
else
{
TMC2130Y.shaft_dir(1);
}
}
if (channelLabel == 'Z')
{
if (motorMotorInv)
{
TMC2130Z.shaft_dir(0);
}
else
{
TMC2130Z.shaft_dir(1);
}
}
#endif
}
void StepperControlAxis::setDirectionDown()
@ -550,11 +715,13 @@ bool StepperControlAxis::endStopsReached()
bool StepperControlAxis::endStopMin()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMin) == motorEndStopInv2) && motorEndStopEnbl);
}
bool StepperControlAxis::endStopMax()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMax) == motorEndStopInv2) && motorEndStopEnbl);
}
@ -572,11 +739,13 @@ void StepperControlAxis::setMotorStep()
{
stepIsOn = true;
digitalWrite(pinStep, HIGH);
//digitalWrite(pinStep, HIGH);
(this->*setMotorStepWrite)();
if (pin2Enable)
{
digitalWrite(pin2Step, HIGH);
(this->*setMotorStepWrite2)();
//digitalWrite(pin2Step, HIGH);
}
}
@ -586,10 +755,12 @@ void StepperControlAxis::resetMotorStep()
movementStepDone = true;
digitalWrite(pinStep, LOW);
//(this->*resetMotorStepWrite)();
if (pin2Enable)
{
digitalWrite(pin2Step, LOW);
//(this->*resetMotorStepWrite2)();
}
}
@ -689,4 +860,98 @@ void StepperControlAxis::resetMotorStepWriteDefault()
void StepperControlAxis::resetMotorStepWriteDefault2()
{
digitalWrite(pin2Step, LOW);
}
}
// X step
void StepperControlAxis::setMotorStepWrite54()
{
//PF0
PORTF |= B00000001;
}
void StepperControlAxis::resetMotorStepWrite54()
{
//PF0
PORTF &= B11111110;
}
// X step 2
void StepperControlAxis::setMotorStepWrite26()
{
//PA4
PORTA |= B00010000;
}
void StepperControlAxis::resetMotorStepWrite26()
{
PORTA &= B11101111;
}
// Y step
void StepperControlAxis::setMotorStepWrite60()
{
//PF6
PORTF |= B01000000;
}
void StepperControlAxis::resetMotorStepWrite60()
{
//PF6
PORTF &= B10111111;
}
// Z step
void StepperControlAxis::setMotorStepWrite46()
{
//PL3
PORTL |= B00001000;
}
void StepperControlAxis::resetMotorStepWrite46()
{
//PL3
PORTL &= B11110111;
}
#if defined(FARMDUINO_EXP_V20)
//// TMC2130 Functions
void StepperControlAxis::setMotorStepWriteTMC2130()
{
// TMC2130 works on each edge of the step pulse,
// so instead of setting the step bit,
// toggle the bit here
if (!tmcStep)
{
digitalWrite(pinStep, HIGH);
}
else
{
digitalWrite(pinStep, LOW);
}
}
void StepperControlAxis::setMotorStepWriteTMC2130_2()
{
if (!tmcStep2)
{
digitalWrite(pin2Step, HIGH);
}
else
{
digitalWrite(pin2Step, LOW);
}
}
void StepperControlAxis::resetMotorStepWriteTMC2130()
{
}
void StepperControlAxis::resetMotorStepWriteTMC2130_2()
{
}
#endif

16
src/StepperControlAxis.h
View File

@ -78,11 +78,13 @@ public:
char channelLabel;
bool movementStarted;
protected:
#if defined(FARMDUINO_EXP_V20)
void initTMC2130A();
#endif
private:
int lastCalcLog = 0;
bool debugPrint = false;
void init();
// pin settings primary motor
int pinStep;
@ -168,6 +170,16 @@ protected:
void setMotorStepWrite46();
void resetMotorStepWrite46();
#if defined(FARMDUINO_EXP_V20)
void setMotorStepWriteTMC2130();
void setMotorStepWriteTMC2130_2();
void resetMotorStepWriteTMC2130();
void resetMotorStepWriteTMC2130_2();
bool tmcStep = true;
bool tmcStep2 = true;
#endif
};
#endif /* STEPPERCONTROLAXIS_H_ */

817
src/StepperControlAxisA4988.cpp
View File

@ -1,817 +0,0 @@
#include "StepperControlAxisA4988.h"
StepperControlAxisA4988::StepperControlAxisA4988()
{
lastCalcLog = 0;
pinStep = 0;
pinDirection = 0;
pinEnable = 0;
pin2Step = 0;
pin2Direction = 0;
pin2Enable = 0;
pinMin = 0;
pinMax = 0;
axisActive = false;
coordSourcePoint = 0;
coordCurrentPoint = 0;
coordDestinationPoint = 0;
coordHomeAxis = 0;
movementUp = false;
movementToHome = false;
movementAccelerating = false;
movementDecelerating = false;
movementCruising = false;
movementCrawling = false;
movementMotorActive = false;
movementMoving = false;
stepIsOn = false;
setMotorStepWrite = &StepperControlAxisA4988::setMotorStepWriteDefault;
setMotorStepWrite2 = &StepperControlAxisA4988::setMotorStepWriteDefault2;
resetMotorStepWrite = &StepperControlAxisA4988::resetMotorStepWriteDefault;
resetMotorStepWrite2 = &StepperControlAxisA4988::resetMotorStepWriteDefault2;
}
void StepperControlAxisA4988::test()
{
Serial.print("R99");
Serial.print(" cur loc = ");
Serial.print(coordCurrentPoint);
//Serial.print(" enc loc = ");
//Serial.print(coordEncoderPoint);
//Serial.print(" cons steps missed = ");
//Serial.print(label);
//Serial.print(consMissedSteps);
Serial.print("\r\n");
}
unsigned int StepperControlAxisA4988::calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec)
{
int newSpeed = 0;
long curPos = abs(currentPosition);
long staPos;
long endPos;
movementAccelerating = false;
movementDecelerating = false;
movementCruising = false;
movementCrawling = false;
movementMoving = false;
/*
if (abs(sourcePosition) < abs(destinationPosition))
{
staPos = abs(sourcePosition);
endPos = abs(destinationPosition);
}
else
{
staPos = abs(destinationPosition);
endPos = abs(sourcePosition);
}
*/
// Set the possible negative coordinates to all positive numbers
// so the calculation code still works after the changes
staPos = 0;
endPos = abs(destinationPosition - sourcePosition);
if (sourcePosition < destinationPosition)
{
curPos = currentPosition - sourcePosition;
}
else
{
curPos = currentPosition - destinationPosition;
}
unsigned long halfway = ((endPos - staPos) / 2) + staPos;
//unsigned long halfway = ((destinationPosition - sourcePosition) / 2) + sourcePosition;
// Set the homing speed if the position would be out of bounds
if (
(curPos < staPos || curPos > endPos)
// ||
// Also limit the speed to a crawl when the move would pass the home position
// (sourcePosition > 0 && destinationPosition < 0) || (sourcePosition < 0 && destinationPosition > 0)
// (!motorHomeIsUp && currentPosition <= 0) || (motorHomeIsUp && currentPosition >= 0) ||)
)
{
newSpeed = motorSpeedHome;
//newSpeed = minSpeed;
movementCrawling = true;
movementMoving = true;
}
else
{
if (curPos >= staPos && curPos <= halfway)
{
// accelerating
if (curPos > (staPos + stepsAccDec))
{
// now beyond the accelleration point to go full speed
newSpeed = maxSpeed + 1;
movementCruising = true;
movementMoving = true;
}
else
{
// speeding up, increase speed linear within the first period
newSpeed = (1.0 * (curPos - staPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
movementAccelerating = true;
movementMoving = true;
}
}
else
{
// decelerating
if (curPos < (endPos - stepsAccDec))
{
// still before the deceleration point so keep going at full speed
newSpeed = maxSpeed + 2;
movementCruising = true;
movementMoving = true;
}
else
{
// speeding up, increase speed linear within the first period
newSpeed = (1.0 * (endPos - curPos) / stepsAccDec * (maxSpeed - minSpeed)) + minSpeed;
movementDecelerating = true;
movementMoving = true;
}
}
}
if (debugPrint && (millis() - lastCalcLog > 1000))
{
lastCalcLog = millis();
Serial.print("R99");
Serial.print(" sta ");
Serial.print(staPos);
Serial.print(" cur ");
Serial.print(curPos);
Serial.print(" end ");
Serial.print(endPos);
Serial.print(" half ");
Serial.print(halfway);
Serial.print(" len ");
Serial.print(stepsAccDec);
Serial.print(" min ");
Serial.print(minSpeed);
Serial.print(" max ");
Serial.print(maxSpeed);
Serial.print(" spd ");
Serial.print(" ");
Serial.print(newSpeed);
Serial.print("\r\n");
}
// Return the calculated speed, in steps per second
return newSpeed;
}
void StepperControlAxisA4988::checkAxisDirection()
{
if (coordHomeAxis)
{
// When home is active, the direction is fixed
movementUp = motorHomeIsUp;
movementToHome = true;
}
else
{
// For normal movement, move in direction of destination
movementUp = (coordCurrentPoint < coordDestinationPoint);
movementToHome = (abs(coordCurrentPoint) >= abs(coordDestinationPoint));
}
if (coordCurrentPoint == 0)
{
// Go slow when theoretical end point reached but there is no end stop siganl
axisSpeed = motorSpeedMin;
}
}
void StepperControlAxisA4988::setDirectionAxis()
{
if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)))
{
setDirectionUp();
}
else
{
setDirectionDown();
}
}
void StepperControlAxisA4988::checkMovement()
{
checkAxisDirection();
// Handle movement if destination is not already reached or surpassed
if (
(
(coordDestinationPoint > coordSourcePoint && coordCurrentPoint < coordDestinationPoint) ||
(coordDestinationPoint < coordSourcePoint && coordCurrentPoint > coordDestinationPoint) ||
coordHomeAxis) &&
axisActive)
{
// home or destination not reached, keep moving
// If end stop reached or the encoder doesn't move anymore, stop moving motor, otherwise set the timing for the next step
if ((coordHomeAxis && !endStopAxisReached(false)) || (!coordHomeAxis && !endStopAxisReached(!movementToHome)))
{
// Get the axis speed, in steps per second
axisSpeed = calculateSpeed(coordSourcePoint, coordCurrentPoint, coordDestinationPoint,
motorSpeedMin, motorSpeedMax, motorStepsAcc);
// // Set the moments when the step is set to true and false
// if (axisSpeed > 0)
// {
// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
// stepOnTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed / 2);
// stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed);
// }
}
else
{
axisActive = false;
}
}
else
{
// Destination or home reached. Deactivate the axis.
axisActive = false;
}
// If end stop for home is active, set the position to zero
if (endStopAxisReached(false))
{
coordCurrentPoint = 0;
}
}
void StepperControlAxisA4988::incrementTick()
{
if (axisActive)
{
moveTicks++;
//moveTicks+=3;
}
}
void StepperControlAxisA4988::checkTiming()
{
//int i;
// moveTicks++;
if (stepIsOn)
{
if (moveTicks >= stepOffTick)
{
// Negative flank for the steps
resetMotorStep();
setTicks();
//stepOnTick = moveTicks + (500000.0 / motorInterruptSpeed / axisSpeed);
}
}
else
{
if (axisActive)
{
if (moveTicks >= stepOnTick)
{
// Positive flank for the steps
setStepAxis();
//stepOffTick = moveTicks + (1000000.0 / motorInterruptSpeed / axisSpeed);
}
}
}
}
void StepperControlAxisA4988::setTicks()
{
// Take the requested speed (steps / second) and divide by the interrupt speed (interrupts per seconde)
// This gives the number of interrupts (called ticks here) before the pulse needs to be set for the next step
stepOnTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed / 2);
stepOffTick = moveTicks + (1000.0 * 1000.0 / motorInterruptSpeed / axisSpeed);
}
void StepperControlAxisA4988::setStepAxis()
{
stepIsOn = true;
if (movementUp)
{
coordCurrentPoint++;
}
else
{
coordCurrentPoint--;
}
// set a step on the motors
setMotorStep();
}
bool StepperControlAxisA4988::endStopAxisReached(bool movement_forward)
{
bool min_endstop = false;
bool max_endstop = false;
bool invert = false;
if (motorEndStopInv)
{
invert = true;
}
// for the axis to check, retrieve the end stop status
if (!invert)
{
min_endstop = endStopMin();
max_endstop = endStopMax();
}
else
{
min_endstop = endStopMax();
max_endstop = endStopMin();
}
// if moving forward, only check the end stop max
// for moving backwards, check only the end stop min
if ((!movement_forward && min_endstop) || (movement_forward && max_endstop))
{
return 1;
}
return 0;
}
void StepperControlAxisA4988::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2)
{
pinStep = step;
pinDirection = dir;
pinEnable = enable;
pin2Step = step2;
pin2Direction = dir2;
pin2Enable = enable2;
pinMin = min;
pinMax = max;
}
void StepperControlAxisA4988::loadMotorSettings(
long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv,
bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl,
bool stopAtHome, long maxSize, bool stopAtMax)
{
motorSpeedMax = speedMax;
motorSpeedMin = speedMin;
motorSpeedHome = speedHome;
motorStepsAcc = stepsAcc;
motorTimeOut = timeOut;
motorHomeIsUp = homeIsUp;
motorMotorInv = motorInv;
motorEndStopInv = endStInv;
motorEndStopInv2 = endStInv2;
motorEndStopEnbl = endStEnbl;
motorInterruptSpeed = interruptSpeed;
motorMotor2Enl = motor2Enbl;
motorMotor2Inv = motor2Inv;
motorStopAtHome = stopAtHome;
motorMaxSize = maxSize;
motorStopAtMax = stopAtMax;
if (pinStep == 54)
{
setMotorStepWrite = &StepperControlAxisA4988::setMotorStepWrite54;
resetMotorStepWrite = &StepperControlAxisA4988::resetMotorStepWrite54;
}
if (pinStep == 60)
{
setMotorStepWrite = &StepperControlAxisA4988::setMotorStepWrite60;
resetMotorStepWrite = &StepperControlAxisA4988::resetMotorStepWrite60;
}
if (pinStep == 46)
{
setMotorStepWrite = &StepperControlAxisA4988::setMotorStepWrite46;
resetMotorStepWrite = &StepperControlAxisA4988::resetMotorStepWrite46;
}
if (pin2Step == 26)
{
setMotorStepWrite2 = &StepperControlAxisA4988::setMotorStepWrite26;
resetMotorStepWrite2 = &StepperControlAxisA4988::resetMotorStepWrite26;
}
}
bool StepperControlAxisA4988::loadCoordinates(long sourcePoint, long destinationPoint, bool home)
{
coordSourcePoint = sourcePoint;
coordCurrentPoint = sourcePoint;
coordDestinationPoint = destinationPoint;
coordHomeAxis = home;
bool changed = false;
// Limit normal movement to the home position
if (motorStopAtHome)
{
if (!motorHomeIsUp && coordDestinationPoint < 0)
{
coordDestinationPoint = 0;
changed = true;
}
if (motorHomeIsUp && coordDestinationPoint > 0)
{
coordDestinationPoint = 0;
changed = true;
}
}
// limit the maximum size the bot can move, when there is a size present
if (motorMaxSize > 0 && motorStopAtMax)
{
if (abs(coordDestinationPoint) > abs(motorMaxSize))
{
if (coordDestinationPoint < 0)
{
coordDestinationPoint = -abs(motorMaxSize);
changed = true;
}
else
{
coordDestinationPoint = abs(motorMaxSize);
changed = true;
}
}
}
// Initialize movement variables
moveTicks = 0;
axisActive = true;
return changed;
}
void StepperControlAxisA4988::enableMotor()
{
digitalWrite(pinEnable, LOW);
if (motorMotor2Enl)
{
digitalWrite(pin2Enable, LOW);
}
movementMotorActive = true;
}
void StepperControlAxisA4988::disableMotor()
{
digitalWrite(pinEnable, HIGH);
if (motorMotor2Enl)
{
digitalWrite(pin2Enable, HIGH);
}
movementMotorActive = false;
}
void StepperControlAxisA4988::setDirectionUp()
{
if (motorMotorInv)
{
digitalWrite(pinDirection, LOW);
}
else
{
digitalWrite(pinDirection, HIGH);
}
if (motorMotor2Enl && motorMotor2Inv)
{
digitalWrite(pin2Direction, LOW);
}
else
{
digitalWrite(pin2Direction, HIGH);
}
}
void StepperControlAxisA4988::setDirectionDown()
{
if (motorMotorInv)
{
digitalWrite(pinDirection, HIGH);
}
else
{
digitalWrite(pinDirection, LOW);
}
if (motorMotor2Enl && motorMotor2Inv)
{
digitalWrite(pin2Direction, HIGH);
}
else
{
digitalWrite(pin2Direction, LOW);
}
}
void StepperControlAxisA4988::setMovementUp()
{
movementUp = true;
}
void StepperControlAxisA4988::setMovementDown()
{
movementUp = false;
}
void StepperControlAxisA4988::setDirectionHome()
{
if (motorHomeIsUp)
{
setDirectionUp();
setMovementUp();
}
else
{
setDirectionDown();
setMovementDown();
}
}
void StepperControlAxisA4988::setDirectionAway()
{
if (motorHomeIsUp)
{
setDirectionDown();
setMovementDown();
}
else
{
setDirectionUp();
setMovementUp();
}
}
unsigned long StepperControlAxisA4988::getLength(long l1, long l2)
{
if (l1 > l2)
{
return l1 - l2;
}
else
{
return l2 - l1;
}
}
bool StepperControlAxisA4988::endStopsReached()
{
return ((digitalRead(pinMin) == motorEndStopInv2) || (digitalRead(pinMax) == motorEndStopInv2)) && motorEndStopEnbl;
}
bool StepperControlAxisA4988::endStopMin()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMin) == motorEndStopInv2) && motorEndStopEnbl);
}
bool StepperControlAxisA4988::endStopMax()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMax) == motorEndStopInv2) && motorEndStopEnbl);
}
bool StepperControlAxisA4988::isAxisActive()
{
return axisActive;
}
void StepperControlAxisA4988::deactivateAxis()
{
axisActive = false;
}
void StepperControlAxisA4988::setMotorStep()
{
stepIsOn = true;
//digitalWrite(pinStep, HIGH);
(this->*setMotorStepWrite)();
if (pin2Enable)
{
(this->*setMotorStepWrite2)();
//digitalWrite(pin2Step, HIGH);
}
}
void StepperControlAxisA4988::resetMotorStep()
{
stepIsOn = false;
movementStepDone = true;
digitalWrite(pinStep, LOW);
//(this->*resetMotorStepWrite)();
if (pin2Enable)
{
digitalWrite(pin2Step, LOW);
//(this->*resetMotorStepWrite2)();
}
}
bool StepperControlAxisA4988::pointReached(long currentPoint, long destinationPoint)
{
return (destinationPoint == currentPoint);
}
long StepperControlAxisA4988::currentPosition()
{
return coordCurrentPoint;
}
void StepperControlAxisA4988::setCurrentPosition(long newPos)
{
coordCurrentPoint = newPos;
}
long StepperControlAxisA4988::destinationPosition()
{
return coordDestinationPoint;
}
void StepperControlAxisA4988::setMaxSpeed(long speed)
{
motorSpeedMax = speed;
}
void StepperControlAxisA4988::activateDebugPrint()
{
debugPrint = true;
}
bool StepperControlAxisA4988::isStepDone()
{
return movementStepDone;
}
void StepperControlAxisA4988::resetStepDone()
{
movementStepDone = false;
}
bool StepperControlAxisA4988::movingToHome()
{
return movementToHome;
}
bool StepperControlAxisA4988::movingUp()
{
return movementUp;
}
bool StepperControlAxisA4988::isAccelerating()
{
return movementAccelerating;
}
bool StepperControlAxisA4988::isDecelerating()
{
return movementDecelerating;
}
bool StepperControlAxisA4988::isCruising()
{
return movementCruising;
}
bool StepperControlAxisA4988::isCrawling()
{
return movementCrawling;
}
bool StepperControlAxisA4988::isMotorActive()
{
return movementMotorActive;
}
/// Functions for pin writing using alternative method
// Pin write default functions
void StepperControlAxisA4988::setMotorStepWriteDefault()
{
digitalWrite(pinStep, HIGH);
}
void StepperControlAxisA4988::setMotorStepWriteDefault2()
{
digitalWrite(pin2Step, HIGH);
}
void StepperControlAxisA4988::resetMotorStepWriteDefault()
{
digitalWrite(pinStep, LOW);
}
void StepperControlAxisA4988::resetMotorStepWriteDefault2()
{
digitalWrite(pin2Step, LOW);
}
// X step
void StepperControlAxisA4988::setMotorStepWrite54()
{
//PF0
PORTF |= B00000001;
}
void StepperControlAxisA4988::resetMotorStepWrite54()
{
//PF0
PORTF &= B11111110;
}
// X step 2
void StepperControlAxisA4988::setMotorStepWrite26()
{
//PA4
PORTA |= B00010000;
}
void StepperControlAxisA4988::resetMotorStepWrite26()
{
PORTA &= B11101111;
}
// Y step
void StepperControlAxisA4988::setMotorStepWrite60()
{
//PF6
PORTF |= B01000000;
}
void StepperControlAxisA4988::resetMotorStepWrite60()
{
//PF6
PORTF &= B10111111;
}
// Z step
void StepperControlAxisA4988::setMotorStepWrite46()
{
//PL3
PORTL |= B00001000;
}
void StepperControlAxisA4988::resetMotorStepWrite46()
{
//PL3
PORTL &= B11110111;
}

173
src/StepperControlAxisA4988.h
View File

@ -1,173 +0,0 @@
/*
* StepperControlAxisA4988.h
*
* Created on: 2019-02-28
* Author: Tim Evers
*/
#ifndef STEPPERCONTROLAXISA4988_H_
#define STEPPERCONTROLAXISA4988_H_
#include "Arduino.h"
#include "CurrentState.h"
#include "ParameterList.h"
#include "pins.h"
#include "StepperControlEncoder.h"
#include "Config.h"
#include <stdio.h>
#include <stdlib.h>
#include "StepperControlAxis.h"
class StepperControlAxisA4988 : public StepperControlAxis
{
public:
StepperControlAxisA4988();
void loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2);
void loadMotorSettings(long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl, bool stopAtHome, long maxSize, bool stopAtMax);
bool loadCoordinates(long sourcePoint, long destinationPoint, bool home);
void setMaxSpeed(long speed);
void enableMotor();
void disableMotor();
void checkMovement();
void incrementTick();
void checkTiming();
void setTicks();
bool isAxisActive();
void deactivateAxis();
bool isAccelerating();
bool isDecelerating();
bool isCruising();
bool isCrawling();
bool isMotorActive();
bool isMoving();
bool endStopMin();
bool endStopMax();
bool endStopsReached();
bool endStopAxisReached(bool movement_forward);
long currentPosition();
void setCurrentPosition(long newPos);
long destinationPosition();
void setStepAxis();
void setMotorStep();
void resetMotorStep();
void setDirectionUp();
void setDirectionDown();
void setDirectionHome();
void setDirectionAway();
void setDirectionAxis();
void setMovementUp();
void setMovementDown();
bool movingToHome();
bool movingUp();
bool isStepDone();
void resetStepDone();
void activateDebugPrint();
void test();
char channelLabel;
bool movementStarted;
private:
int lastCalcLog = 0;
bool debugPrint = false;
// pin settings primary motor
int pinStep;
int pinDirection;
int pinEnable;
// pin settings primary motor
int pin2Step;
int pin2Direction;
int pin2Enable;
// pin settings primary motor
int pinMin;
int pinMax;
// motor settings
bool motorEndStopInv; // switch places of end stops
bool motorEndStopInv2; // invert input (true/normal open, falce/normal closed) of end stops
bool motorEndStopEnbl; // enable the use of the end stops
// motor settings
long motorSpeedMax; // maximum speed in steps per second
long motorSpeedMin; // minimum speed in steps per second
long motorSpeedHome; // homing speed in steps per second
long motorStepsAcc; // number of steps used for acceleration
long motorTimeOut; // timeout in seconds
bool motorHomeIsUp; // direction to move when reached 0 on axis but no end stop detected while homing
bool motorMotorInv; // invert motor direction
bool motorMotor2Enl; // enable secondary motor
bool motorMotor2Inv; // invert secondary motor direction
long motorInterruptSpeed; // period of interrupt in micro seconds
bool motorStopAtHome; // stop at home position or also use other side of the axis
long motorMaxSize; // maximum size of the axis in steps
bool motorStopAtMax; // stop at the maximum size
// coordinates
long coordSourcePoint; // all coordinated in steps
long coordCurrentPoint;
long coordDestinationPoint;
bool coordHomeAxis; // homing command
// movement handling
unsigned long movementLength;
unsigned long maxLenth;
unsigned long moveTicks;
unsigned long stepOnTick;
unsigned long stepOffTick;
unsigned long axisSpeed;
bool axisActive;
bool movementUp;
bool movementToHome;
bool movementStepDone;
bool movementAccelerating;
bool movementDecelerating;
bool movementCruising;
bool movementCrawling;
bool movementMotorActive;
bool movementMoving;
bool stepIsOn;
void step(long &currentPoint, unsigned long steps, long destinationPoint);
bool pointReached(long currentPoint, long destinationPoint);
unsigned int calculateSpeed(long sourcePosition, long currentPosition, long destinationPosition, long minSpeed, long maxSpeed, long stepsAccDec);
unsigned long getLength(long l1, long l2);
void checkAxisDirection();
void (StepperControlAxisA4988::*setMotorStepWrite)();
void (StepperControlAxisA4988::*setMotorStepWrite2)();
void (StepperControlAxisA4988::*resetMotorStepWrite)();
void (StepperControlAxisA4988::*resetMotorStepWrite2)();
void setMotorStepWriteDefault();
void setMotorStepWriteDefault2();
void resetMotorStepWriteDefault();
void resetMotorStepWriteDefault2();
void setMotorStepWrite54();
void resetMotorStepWrite54();
void setMotorStepWrite26();
void resetMotorStepWrite26();
void setMotorStepWrite60();
void resetMotorStepWrite60();
void setMotorStepWrite46();
void resetMotorStepWrite46();
};
#endif /* STEPPERCONTROLAXISA4988_H_ */

168
src/StepperControlAxisTMC2130.cpp
View File

@ -1,168 +0,0 @@
#include "StepperControlAxisTMC2130.h"
StepperControlAxisTMC2130::StepperControlAxisTMC2130()
{
init();
}
void StepperControlAxisTMC2130::initTMC2130A()
{
TMC2130A.begin(); // Initiate pins and registeries
TMC2130A.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130A.stealthChop(1); // Enable extremely quiet stepping
}
void StepperControlAxisTMC2130::initTMC2130B()
{
TMC2130B.begin(); // Initiate pins and registeries
TMC2130B.SilentStepStick2130(600); // Set stepper current to 600mA
TMC2130B.stealthChop(1); // Enable extremely quiet stepping
}
void StepperControlAxisTMC2130::test()
{
//Serial.print("R99");
//Serial.print(" cur loc = ");
//Serial.print(coordCurrentPoint);
//Serial.print("\r\n");
}
void StepperControlAxisTMC2130::loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2)
{
pinStep = step;
pinDirection = dir;
pinEnable = enable;
pin2Step = step2;
pin2Direction = dir2;
pin2Enable = enable2;
pinMin = min;
pinMax = max;
}
void StepperControlAxisTMC2130::loadMotorSettings(
long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv,
bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl,
bool stopAtHome, long maxSize, bool stopAtMax)
{
motorSpeedMax = speedMax;
motorSpeedMin = speedMin;
motorSpeedHome = speedHome;
motorStepsAcc = stepsAcc;
motorTimeOut = timeOut;
motorHomeIsUp = homeIsUp;
motorMotorInv = motorInv;
motorEndStopInv = endStInv;
motorEndStopInv2 = endStInv2;
motorEndStopEnbl = endStEnbl;
motorInterruptSpeed = interruptSpeed;
motorMotor2Enl = motor2Enbl;
motorMotor2Inv = motor2Inv;
motorStopAtHome = stopAtHome;
motorMaxSize = maxSize;
motorStopAtMax = stopAtMax;
}
void StepperControlAxisTMC2130::enableMotor()
{
digitalWrite(pinEnable, LOW);
if (motorMotor2Enl)
{
digitalWrite(pin2Enable, LOW);
}
movementMotorActive = true;
}
void StepperControlAxisTMC2130::disableMotor()
{
digitalWrite(pinEnable, HIGH);
if (motorMotor2Enl)
{
digitalWrite(pin2Enable, HIGH);
}
movementMotorActive = false;
}
void StepperControlAxisTMC2130::setDirectionUp()
{
if (motorMotorInv)
{
TMC2130A.shaft_dir(0);
}
else
{
TMC2130A.shaft_dir(1);
}
if (motorMotor2Enl && motorMotor2Inv)
{
TMC2130B.shaft_dir(0);
}
else
{
TMC2130B.shaft_dir(1);
}
}
void StepperControlAxisTMC2130::setDirectionDown()
{
if (motorMotorInv)
{
TMC2130A.shaft_dir(1);
}
else
{
TMC2130A.shaft_dir(0);
}
if (motorMotor2Enl && motorMotor2Inv)
{
TMC2130B.shaft_dir(1);
}
else
{
TMC2130B.shaft_dir(0);
}
}
void StepperControlAxisTMC2130::setMotorStep()
{
stepIsOn = true;
// TMC2130 works on each edge, so instead of setting the
// step bit, toggle the bit here
Serial.print("*");
if (!tmcStep)
{
digitalWrite(pinStep, HIGH);
if (pin2Enable)
{
digitalWrite(pin2Step, HIGH);
}
}
else
{
digitalWrite(pinStep, LOW);
if (pin2Enable)
{
digitalWrite(pin2Step, LOW);
}
}
tmcStep = !tmcStep;
}
void StepperControlAxisTMC2130::resetMotorStep()
{
// No reset needed for TMC2130
stepIsOn = false;
movementStepDone = true;
}

59
src/StepperControlAxisTMC2130.h
View File

@ -1,59 +0,0 @@
/*
* StepperControlAxisTMC2130.h
*
* Created on: 2019-02-28
* Author: Tim Evers
*/
#ifndef STEPPERCONTROLAXISTMC2130_H_
#define STEPPERCONTROLAXISTMC2130_H_
#include "Arduino.h"
#include "CurrentState.h"
#include "ParameterList.h"
#include "pins.h"
#include "StepperControlEncoder.h"
#include "Config.h"
#include <stdio.h>
#include <stdlib.h>
#include "StepperControlAxis.h"
#include <TMC2130Stepper.h>
class StepperControlAxisTMC2130 : public StepperControlAxis
{
public:
StepperControlAxisTMC2130();
void initTMC2130A();
void initTMC2130B();
void loadPinNumbers(int step, int dir, int enable, int min, int max, int step2, int dir2, int enable2);
void loadMotorSettings(long speedMax, long speedMin, long speedHome, long stepsAcc, long timeOut, bool homeIsUp, bool motorInv, bool endStInv, bool endStInv2, long interruptSpeed, bool motor2Enbl, bool motor2Inv, bool endStEnbl, bool stopAtHome, long maxSize, bool stopAtMax);
void enableMotor();
void disableMotor();
void setMotorStep();
void resetMotorStep();
void setDirectionUp();
void setDirectionDown();
void test();
char channelLabel;
bool movementStarted;
private:
TMC2130Stepper TMC2130A = TMC2130Stepper(X_ENABLE_PIN, X_DIR_PIN, X_STEP_PIN, X_CHIP_SELECT);
TMC2130Stepper TMC2130B = TMC2130Stepper(E_ENABLE_PIN, E_DIR_PIN, E_STEP_PIN, E_CHIP_SELECT);
bool tmcStep = false;
};
#endif /* STEPPERCONTROLAXISTMC2130_H_ */

4
src/src.vcxproj
View File

@ -123,8 +123,6 @@
<ClCompile Include="StatusList.cpp" />
<ClCompile Include="StepperControl.cpp" />
<ClCompile Include="StepperControlAxis.cpp" />
<ClCompile Include="StepperControlAxisA4988.cpp" />
<ClCompile Include="StepperControlAxisTMC2130.cpp" />
<ClCompile Include="StepperControlEncoder.cpp" />
<ClCompile Include="TimerOne.cpp" />
</ItemGroup>
@ -171,8 +169,6 @@
<ClInclude Include="StatusList.h" />
<ClInclude Include="StepperControl.h" />
<ClInclude Include="StepperControlAxis.h" />
<ClInclude Include="StepperControlAxisA4988.h" />
<ClInclude Include="StepperControlAxisTMC2130.h" />
<ClInclude Include="StepperControlEncoder.h" />
<ClInclude Include="TimerOne.h" />
<ClInclude Include="__vm\.src.vsarduino.h" />

12
src/src.vcxproj.filters
View File

@ -135,12 +135,6 @@
<ClCompile Include="F09Handler.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="StepperControlAxisA4988.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="StepperControlAxisTMC2130.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="GCodeProcessor.h">
@ -278,11 +272,5 @@
<ClInclude Include="Board.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="StepperControlAxisA4988.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="StepperControlAxisTMC2130.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
</Project>