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

Fixing alt coords at max length

pull/118/head
Tim Evers 2018-03-29 21:37:01 +02:00
parent 59e6ee74fa
commit 04f71c3977
12 changed files with 2156 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/Board.h
View File

@ -1,3 +1,24 @@
//#define RAMPS_V14
//#define FARMDUINO_V10
#define FARMDUINO_V14
#ifndef FARMBOT_BOARD_ID
//#define RAMPS_V14
//#define FARMDUINO_V10
//#define FARMDUINO_V14
#define FARMDUINO_EXP_V20
#else
#undef RAMPS_V14
#undef FARMDUINO_V10
#undef FARMDUINO_V14
#if FARMBOT_BOARD_ID == 0
#define RAMPS_V14
#elif FARMBOT_BOARD_ID == 1
#define FARMDUINO_V10
#elif FARMBOT_BOARD_ID == 2
#define FARMDUINO_V14
#elif FARMBOT_BOARD_ID == 3
#define FARMDUINO_EXP_V20
#endif
#endif

10
src/Config.h
View File

@ -224,13 +224,17 @@ enum MdlSpiEncoders
#endif /* CONFIG_H_ */
#if defined(RAMPS_V14) && !defined(SOFTWARE_VERSION)
#define SOFTWARE_VERSION "6.3.0.R\0"
#define SOFTWARE_VERSION "6.4.2.R\0"
#endif
#if defined(FARMDUINO_V10) && !defined(SOFTWARE_VERSION)
#define SOFTWARE_VERSION "6.3.0.F\0"
#define SOFTWARE_VERSION "6.4.2.F\0"
#endif
#if defined(FARMDUINO_V14) && !defined(SOFTWARE_VERSION)
#define SOFTWARE_VERSION "6.3.0.G\0"
#define SOFTWARE_VERSION "6.4.2.G\0"
#endif
#if defined(FARMDUINO_EXP_V20) && !defined(SOFTWARE_VERSION)
#define SOFTWARE_VERSION "6.4.2.E\0"
#endif

12
src/StepperControl.cpp
View File

@ -314,7 +314,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
{
Serial.print(COMM_REPORT_COORD_CHANGED_X);
Serial.print(" X");
Serial.print(axisX.destinationPosition());
Serial.print(axisX.destinationPosition() / stepsPerMm[0]);
CurrentState::getInstance()->printQAndNewLine();
}
@ -322,7 +322,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
{
Serial.print(COMM_REPORT_COORD_CHANGED_Y);
Serial.print(" Y");
Serial.print(axisY.destinationPosition());
Serial.print(axisY.destinationPosition() / stepsPerMm[1]);
CurrentState::getInstance()->printQAndNewLine();
}
@ -330,7 +330,7 @@ int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double
{
Serial.print(COMM_REPORT_COORD_CHANGED_Z);
Serial.print(" Z");
Serial.print(axisZ.destinationPosition());
Serial.print(axisZ.destinationPosition() / stepsPerMm[2]);
CurrentState::getInstance()->printQAndNewLine();
}
// Prepare for movement
@ -1323,9 +1323,9 @@ void StepperControl::loadMotorSettings()
CurrentState::getInstance()->setStepsPerMm(stepsPerMm[0], stepsPerMm[1], stepsPerMm[2]);
axisX.loadMotorSettings(speedMax[0], speedMin[0], speedHome[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], endStInv2[0], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[0], motor2Inv[0], endStEnbl[0], motorStopAtHome[0], motorMaxSize[0], motorStopAtMax[0]);
axisY.loadMotorSettings(speedMax[1], speedMin[1], speedHome[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], endStInv2[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1], motorStopAtHome[1], motorMaxSize[1], motorStopAtMax[1]);
axisZ.loadMotorSettings(speedMax[2], speedMin[2], speedHome[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], endStInv2[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2], motorStopAtHome[2], motorMaxSize[2], motorStopAtMax[2]);
axisX.loadMotorSettings(speedMax[0], speedMin[0], speedHome[0], stepsAcc[0], timeOut[0], homeIsUp[0], motorInv[0], endStInv[0], endStInv2[0], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[0], motor2Inv[0], endStEnbl[0], motorStopAtHome[0], motorMaxSize[0] *= stepsPerMm[0], motorStopAtMax[0]);
axisY.loadMotorSettings(speedMax[1], speedMin[1], speedHome[1], stepsAcc[1], timeOut[1], homeIsUp[1], motorInv[1], endStInv[1], endStInv2[1], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[1], motor2Inv[1], endStEnbl[1], motorStopAtHome[1], motorMaxSize[1] *= stepsPerMm[1], motorStopAtMax[1]);
axisZ.loadMotorSettings(speedMax[2], speedMin[2], speedHome[2], stepsAcc[2], timeOut[2], homeIsUp[2], motorInv[2], endStInv[2], endStInv2[2], MOVEMENT_INTERRUPT_SPEED, motor2Enbl[2], motor2Inv[2], endStEnbl[2], motorStopAtHome[2], motorMaxSize[2] *= stepsPerMm[2], motorStopAtMax[2]);
primeMotors();
}

2
src/StepperControl.h
View File

@ -19,6 +19,8 @@
#include <stdlib.h>
#include "Command.h"
#include "StepperControlAxisA4988.h"
#include "StepperControlAxisTMC2130.h"
class StepperControl
{

137
src/StepperControlAxis.cpp
View File

@ -33,11 +33,6 @@ StepperControlAxis::StepperControlAxis()
stepIsOn = false;
setMotorStepWrite = &StepperControlAxis::setMotorStepWriteDefault;
setMotorStepWrite2 = &StepperControlAxis::setMotorStepWriteDefault2;
resetMotorStepWrite = &StepperControlAxis::resetMotorStepWriteDefault;
resetMotorStepWrite2 = &StepperControlAxis::resetMotorStepWriteDefault2;
}
void StepperControlAxis::test()
@ -45,11 +40,6 @@ 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");
}
@ -69,20 +59,6 @@ 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;
@ -99,19 +75,13 @@ 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;
}
@ -250,15 +220,6 @@ 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
{
@ -289,19 +250,13 @@ 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
@ -310,10 +265,8 @@ void StepperControlAxis::checkTiming()
{
if (moveTicks >= stepOnTick)
{
// Positive flank for the steps
setStepAxis();
//stepOffTick = moveTicks + (1000000.0 / motorInterruptSpeed / axisSpeed);
}
}
}
@ -417,32 +370,6 @@ 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)
@ -617,13 +544,11 @@ 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);
}
@ -641,13 +566,11 @@ void StepperControlAxis::setMotorStep()
{
stepIsOn = true;
//digitalWrite(pinStep, HIGH);
(this->*setMotorStepWrite)();
digitalWrite(pinStep, HIGH);
if (pin2Enable)
{
(this->*setMotorStepWrite2)();
//digitalWrite(pin2Step, HIGH);
digitalWrite(pin2Step, HIGH);
}
}
@ -657,12 +580,10 @@ void StepperControlAxis::resetMotorStep()
movementStepDone = true;
digitalWrite(pinStep, LOW);
//(this->*resetMotorStepWrite)();
if (pin2Enable)
{
digitalWrite(pin2Step, LOW);
//(this->*resetMotorStepWrite2)();
}
}
@ -762,56 +683,4 @@ 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;
}
}

2
src/StepperControlAxis.h
View File

@ -113,7 +113,7 @@ private:
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
long motorMaxSize; // maximum size of the axis in steps
bool motorStopAtMax; // stop at the maximum size
// coordinates

817
src/StepperControlAxisA4988.cpp 100644
View File

@ -0,0 +1,817 @@
#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 100644
View File

@ -0,0 +1,173 @@
/*
* 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 label;
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_ */

817
src/StepperControlAxisTMC2130.cpp 100644
View File

@ -0,0 +1,817 @@
#include "StepperControlAxisTMC2130.h"
StepperControlAxisTMC2130::StepperControlAxisTMC2130()
{
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 = &StepperControlAxisTMC2130::setMotorStepWriteDefault;
setMotorStepWrite2 = &StepperControlAxisTMC2130::setMotorStepWriteDefault2;
resetMotorStepWrite = &StepperControlAxisTMC2130::resetMotorStepWriteDefault;
resetMotorStepWrite2 = &StepperControlAxisTMC2130::resetMotorStepWriteDefault2;
}
void StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::setDirectionAxis()
{
if (((!coordHomeAxis && coordCurrentPoint < coordDestinationPoint) || (coordHomeAxis && motorHomeIsUp)))
{
setDirectionUp();
}
else
{
setDirectionDown();
}
}
void StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::incrementTick()
{
if (axisActive)
{
moveTicks++;
//moveTicks+=3;
}
}
void StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::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 StepperControlAxisTMC2130::setStepAxis()
{
stepIsOn = true;
if (movementUp)
{
coordCurrentPoint++;
}
else
{
coordCurrentPoint--;
}
// set a step on the motors
setMotorStep();
}
bool StepperControlAxisTMC2130::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 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;
if (pinStep == 54)
{
setMotorStepWrite = &StepperControlAxisTMC2130::setMotorStepWrite54;
resetMotorStepWrite = &StepperControlAxisTMC2130::resetMotorStepWrite54;
}
if (pinStep == 60)
{
setMotorStepWrite = &StepperControlAxisTMC2130::setMotorStepWrite60;
resetMotorStepWrite = &StepperControlAxisTMC2130::resetMotorStepWrite60;
}
if (pinStep == 46)
{
setMotorStepWrite = &StepperControlAxisTMC2130::setMotorStepWrite46;
resetMotorStepWrite = &StepperControlAxisTMC2130::resetMotorStepWrite46;
}
if (pin2Step == 26)
{
setMotorStepWrite2 = &StepperControlAxisTMC2130::setMotorStepWrite26;
resetMotorStepWrite2 = &StepperControlAxisTMC2130::resetMotorStepWrite26;
}
}
bool StepperControlAxisTMC2130::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 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)
{
digitalWrite(pinDirection, LOW);
}
else
{
digitalWrite(pinDirection, HIGH);
}
if (motorMotor2Enl && motorMotor2Inv)
{
digitalWrite(pin2Direction, LOW);
}
else
{
digitalWrite(pin2Direction, HIGH);
}
}
void StepperControlAxisTMC2130::setDirectionDown()
{
if (motorMotorInv)
{
digitalWrite(pinDirection, HIGH);
}
else
{
digitalWrite(pinDirection, LOW);
}
if (motorMotor2Enl && motorMotor2Inv)
{
digitalWrite(pin2Direction, HIGH);
}
else
{
digitalWrite(pin2Direction, LOW);
}
}
void StepperControlAxisTMC2130::setMovementUp()
{
movementUp = true;
}
void StepperControlAxisTMC2130::setMovementDown()
{
movementUp = false;
}
void StepperControlAxisTMC2130::setDirectionHome()
{
if (motorHomeIsUp)
{
setDirectionUp();
setMovementUp();
}
else
{
setDirectionDown();
setMovementDown();
}
}
void StepperControlAxisTMC2130::setDirectionAway()
{
if (motorHomeIsUp)
{
setDirectionDown();
setMovementDown();
}
else
{
setDirectionUp();
setMovementUp();
}
}
unsigned long StepperControlAxisTMC2130::getLength(long l1, long l2)
{
if (l1 > l2)
{
return l1 - l2;
}
else
{
return l2 - l1;
}
}
bool StepperControlAxisTMC2130::endStopsReached()
{
return ((digitalRead(pinMin) == motorEndStopInv2) || (digitalRead(pinMax) == motorEndStopInv2)) && motorEndStopEnbl;
}
bool StepperControlAxisTMC2130::endStopMin()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMin) == motorEndStopInv2) && motorEndStopEnbl);
}
bool StepperControlAxisTMC2130::endStopMax()
{
//return ((digitalRead(pinMin) == motorEndStopInv) || (digitalRead(pinMax) == motorEndStopInv));
return ((digitalRead(pinMax) == motorEndStopInv2) && motorEndStopEnbl);
}
bool StepperControlAxisTMC2130::isAxisActive()
{
return axisActive;
}
void StepperControlAxisTMC2130::deactivateAxis()
{
axisActive = false;
}
void StepperControlAxisTMC2130::setMotorStep()
{
stepIsOn = true;
//digitalWrite(pinStep, HIGH);
(this->*setMotorStepWrite)();
if (pin2Enable)
{
(this->*setMotorStepWrite2)();
//digitalWrite(pin2Step, HIGH);
}
}
void StepperControlAxisTMC2130::resetMotorStep()
{
stepIsOn = false;
movementStepDone = true;
digitalWrite(pinStep, LOW);
//(this->*resetMotorStepWrite)();
if (pin2Enable)
{
digitalWrite(pin2Step, LOW);
//(this->*resetMotorStepWrite2)();
}
}
bool StepperControlAxisTMC2130::pointReached(long currentPoint, long destinationPoint)
{
return (destinationPoint == currentPoint);
}
long StepperControlAxisTMC2130::currentPosition()
{
return coordCurrentPoint;
}
void StepperControlAxisTMC2130::setCurrentPosition(long newPos)
{
coordCurrentPoint = newPos;
}
long StepperControlAxisTMC2130::destinationPosition()
{
return coordDestinationPoint;
}
void StepperControlAxisTMC2130::setMaxSpeed(long speed)
{
motorSpeedMax = speed;
}
void StepperControlAxisTMC2130::activateDebugPrint()
{
debugPrint = true;
}
bool StepperControlAxisTMC2130::isStepDone()
{
return movementStepDone;
}
void StepperControlAxisTMC2130::resetStepDone()
{
movementStepDone = false;
}
bool StepperControlAxisTMC2130::movingToHome()
{
return movementToHome;
}
bool StepperControlAxisTMC2130::movingUp()
{
return movementUp;
}
bool StepperControlAxisTMC2130::isAccelerating()
{
return movementAccelerating;
}
bool StepperControlAxisTMC2130::isDecelerating()
{
return movementDecelerating;
}
bool StepperControlAxisTMC2130::isCruising()
{
return movementCruising;
}
bool StepperControlAxisTMC2130::isCrawling()
{
return movementCrawling;
}
bool StepperControlAxisTMC2130::isMotorActive()
{
return movementMotorActive;
}
/// Functions for pin writing using alternative method
// Pin write default functions
void StepperControlAxisTMC2130::setMotorStepWriteDefault()
{
digitalWrite(pinStep, HIGH);
}
void StepperControlAxisTMC2130::setMotorStepWriteDefault2()
{
digitalWrite(pin2Step, HIGH);
}
void StepperControlAxisTMC2130::resetMotorStepWriteDefault()
{
digitalWrite(pinStep, LOW);
}
void StepperControlAxisTMC2130::resetMotorStepWriteDefault2()
{
digitalWrite(pin2Step, LOW);
}
// X step
void StepperControlAxisTMC2130::setMotorStepWrite54()
{
//PF0
PORTF |= B00000001;
}
void StepperControlAxisTMC2130::resetMotorStepWrite54()
{
//PF0
PORTF &= B11111110;
}
// X step 2
void StepperControlAxisTMC2130::setMotorStepWrite26()
{
//PA4
PORTA |= B00010000;
}
void StepperControlAxisTMC2130::resetMotorStepWrite26()
{
PORTA &= B11101111;
}
// Y step
void StepperControlAxisTMC2130::setMotorStepWrite60()
{
//PF6
PORTF |= B01000000;
}
void StepperControlAxisTMC2130::resetMotorStepWrite60()
{
//PF6
PORTF &= B10111111;
}
// Z step
void StepperControlAxisTMC2130::setMotorStepWrite46()
{
//PL3
PORTL |= B00001000;
}
void StepperControlAxisTMC2130::resetMotorStepWrite46()
{
//PL3
PORTL &= B11110111;
}

173
src/StepperControlAxisTMC2130.h 100644
View File

@ -0,0 +1,173 @@
/*
* 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"
class StepperControlAxisTMC2130 : public StepperControlAxis
{
public:
StepperControlAxisTMC2130();
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 label;
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 (StepperControlAxisTMC2130::*setMotorStepWrite)();
void (StepperControlAxisTMC2130::*setMotorStepWrite2)();
void (StepperControlAxisTMC2130::*resetMotorStepWrite)();
void (StepperControlAxisTMC2130::*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 /* STEPPERCONTROLAXISTMC2130_H_ */

133
src/pins.h
View File

@ -65,6 +65,8 @@
#define SERVO_0_PIN 4
#define SERVO_1_PIN 5
#define SERVO_2_PIN 6
#define SERVO_3_PIN 11
#define AUX1_00 0
#define AUX1_01 1
@ -202,7 +204,7 @@
#define SERVO_0_PIN 4
#define SERVO_1_PIN 5
#define SERVO_2_PIN 6
#define SERVO_3_PIN 7
#define SERVO_3_PIN 11
// Encoder X channel A: pin 16, port H1
#define ENC_X_A_PORT PINH
@ -254,3 +256,132 @@
#endif
#if defined(FARMDUINO_EXP_V20)
// X1-AXIS
#define X_STEP_PIN 26 // X1_STEP_PIN
#define X_DIR_PIN 27 // X1_DIR_PIN
#define X_ENABLE_PIN 25 // X1_ENABLE_PIN
#define X_CHIP_SELECT 24 // X1_CHIP_SELECT
#define X_MIN_PIN 69
#define X_MAX_PIN 68
#define X_ENCDR_A 16
#define X_ENCDR_B 17
#define X_ENCDR_A_Q -1 // N/A
#define X_ENCDR_B_Q -1 // N/A
// X2-AXIS
#define E_STEP_PIN 15 // X2_STEP_PIN
#define E_DIR_PIN 30 // X2_DIR_PIN
#define E_ENABLE_PIN 14 // X2_ENABLE_PIN
#define E_CHIP_SELECT 29 // X2_CHIP_SELECT
#define X2_ENCDR_A 22
#define X2_ENCDR_B 39
// Y-AXIS
#define Y_STEP_PIN 32
#define Y_DIR_PIN 33
#define Y_ENABLE_PIN 31
#define Y_CHIP_SELECT 28 // Y_CHIP_SELECT
#define Y_MIN_PIN 67
#define Y_MAX_PIN 66
#define Y_ENCDR_A 23
#define Y_ENCDR_B 24
#define Y_ENCDR_A_Q -1 // N/A
#define Y_ENCDR_B_Q -1 // N/A
// Z-AXIS
#define Z_STEP_PIN 35
#define Z_DIR_PIN 36
#define Z_ENABLE_PIN 34
#define Z_CHIP_SELECT 23 // Z_CHIP_SELECT
#define Z_MIN_PIN 65
#define Z_MAX_PIN 64
#define Z_ENCDR_A 29
#define Z_ENCDR_B 28
#define Z_ENCDR_A_Q -1 // N/A
#define Z_ENCDR_B_Q -1 // N/A
// UTM
#define UTM_C 63 // TOOL VERIFICATION
#define UTM_D 59 // SOIL SENSOR
#define UTM_E -1
#define UTM_F -1
#define UTM_G -1
#define UTM_H -1
#define UTM_I -1
#define UTM_J -1
#define UTM_K -1
#define UTM_L -1
// Available digital pins: 2,3,18,19,38,42,43,44,45,46,47,48,49
// Available analog pins: 0,1,2,3,4,6,7,8
#define LED_PIN 13
// Peripherals
#define LIGHTING_PIN 7
#define WATER_PIN 8
#define VACUUM_PIN 9
#define PERIPHERAL_4_PIN 10
#define PERIPHERAL_5_PIN 12
// Auxiliary motors
#define AUX_STEP_PIN 40
#define AUX_DIR_PIN 41
#define AUX_ENABLE_PIN 37
#define SERVO_0_PIN 4
#define SERVO_1_PIN 5
#define SERVO_2_PIN 6
#define SERVO_3_PIN 7
// Encoder X channel A: pin 16, port H1
#define ENC_X_A_PORT PINH
#define ENC_X_A_BYTE 0x02
// Encoder X channel B: pin 17, port H0
#define ENC_X_B_PORT PINH
#define ENC_X_B_BYTE 0x01
// Encoder X channel A Q (disabled, use LED pin): pin 13, port B7
#define ENC_X_A_Q_PORT PINB
#define ENC_X_A_Q_BYTE 0x80
// Encoder X channel B Q (disabled, use LED pin): pin 13, port B7
#define ENC_X_B_Q_PORT PINB
#define ENC_X_B_Q_BYTE 0x80
// Encoder Y channel A: pin 23, port A1
#define ENC_Y_A_PORT PINA
#define ENC_Y_A_BYTE 0x02
// Encoder Y channel B: pin 24, port A2
#define ENC_Y_B_PORT PINA
#define ENC_Y_B_BYTE 0x04
// Encoder Y channel A Q (disabled, use LED pin): pin 13, port B7
#define ENC_Y_A_Q_PORT PINB
#define ENC_Y_A_Q_BYTE 0x80
// Encoder Y channel B Q (disabled, use LED pin): pin 13, port B7
#define ENC_Y_B_Q_PORT PINB
#define ENC_Y_B_Q_BYTE 0x80
// Encoder Z channel A: pin 29, port A7
#define ENC_Z_A_PORT PINA
#define ENC_Z_A_BYTE 0x80
// Encoder Z channel B: pin 28, port A6
#define ENC_Z_B_PORT PINA
#define ENC_Z_B_BYTE 0x40
// Encoder Z channel A Q (disabled, use LED pin): pin 13, port B7
#define ENC_Z_A_Q_PORT PINB
#define ENC_Z_A_Q_BYTE 0x80
// Encoder Z channel B Q (disabled, use LED pin): pin 13, port B7
#define ENC_Z_B_Q_PORT PINB
#define ENC_Z_B_Q_BYTE 0x80
#endif

2
src/src.vcxproj
View File

@ -47,7 +47,7 @@
<WarningLevel>Level3</WarningLevel>
<Optimization>Disabled</Optimization>
<SDLCheck>true</SDLCheck>
<AdditionalIncludeDirectories>$(ProjectDir)..\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\..\..\Program Files (x86)\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries;$(ProjectDir)..\..\..\..\Documents\Arduino\libraries;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr\;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<AdditionalIncludeDirectories>$(ProjectDir)..\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\SPI\src;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries\EEPROM\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries\Servo\src;$(ProjectDir)..\..\..\..\Program Files (x86)\Arduino\libraries;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\libraries;$(ProjectDir)..\..\..\..\Users\Bro\Documents\Arduino\libraries;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\cores\arduino;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\hardware\avr\1.6.19\variants\mega;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\avr\include\avr\;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.8.1\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.2\include;$(ProjectDir)..\..\..\..\Users\Bro\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\4.9.2-atmel3.5.4-arduino2\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
<ForcedIncludeFiles>$(ProjectDir)__vm\.src.vsarduino.h;%(ForcedIncludeFiles)</ForcedIncludeFiles>
<IgnoreStandardIncludePath>false</IgnoreStandardIncludePath>
<PreprocessorDefinitions>__AVR_ATmega2560__;_VMDEBUG=1;F_CPU=16000000L;ARDUINO=10802;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions)</PreprocessorDefinitions>