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

making classes for axis and encoder more generic

pull/33/head
TimEvWw 2015-11-11 21:09:15 -01:00
parent 3fb23a076d
commit abb469b87d
5 changed files with 174 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

124
src/StepperControl.cpp
View File

@ -15,6 +15,15 @@ StepperControl * StepperControl::getInstance() {
StepperControl::StepperControl() {
// Initialize some variables
motorConsMissedStepsMax[0] = 50;
motorConsMissedStepsMax[1] = 50;
motorConsMissedStepsMax[2] = 50;
motorConsMissedSteps[0] = 0;
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
// Create the axis controllers
axisX = StepperControlAxis();
@ -81,6 +90,10 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
loadMotorSettings();
// load current encoder coordinates
axisX.setCurrentPosition(encoderX.currentPosition());
// if a speed is given in the command, use that instead of the config speed
if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) {
@ -116,6 +129,10 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
destinationPoint[1] = yDest;
destinationPoint[2] = zDest;
motorConsMissedSteps[0] = 0;
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
// Load coordinates into motor control
axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome);
@ -127,7 +144,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
storeEndStops();
reportEndStops();
axisX.setDirectionAxis();
axisY.setDirectionAxis();
axisZ.setDirectionAxis();
@ -140,7 +156,6 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
axisActive[1] = true;
axisActive[2] = true;
axisX.checkMovement();
axisY.checkMovement();
axisZ.checkMovement();
@ -151,24 +166,80 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
// Let the interrupt handle all the movements
while (axisActive[0] || axisActive[1] || axisActive[2]) {
delay(1);
//delay(1);
delayMicroseconds(100);
/**/
encoderX.readEncoder();
//long entX= encoderX.readEncoder();
currentPoint[0] = encoderX.currentPosition();
currentPoint[1] = encoderY.currentPosition();
currentPoint[2] = encoderZ.currentPosition();
axisX.setCurrentPosition(currentPoint[0]);
axisY.setCurrentPosition(currentPoint[1]);
axisZ.setCurrentPosition(currentPoint[2]);
//encoderX.currentPosition();
//encoderY.currentPosition();
//encoderZ.currentPosition();
axisX.checkTiming();
axisY.checkTiming();
axisZ.checkTiming();
encoderX.readEncoder();
// If a step is taken, check to see if the encoder still matcjes the nr of steps
if (axisX.isStepDone()) {
//Serial.print("R99");
//Serial.print(" encoder pos ");
//Serial.print(encoderX.currentPosition());
//Serial.print(" axis pos ");
//Serial.print(axisX.currentPosition());
//Serial.print("\n");
Serial.print("R99");
Serial.print(" missed step ");
Serial.print(motorConsMissedSteps[0]);
Serial.print(" encoder pos ");
Serial.print(encoderX.currentPosition());
Serial.print(" axis pos ");
Serial.print(axisX.currentPosition());
Serial.print("\n");
if (axisX.currentPosition() != encoderX.currentPosition()) {
//motorConsMissedSteps[0]++;
//motorConsMissedSteps[0]++;
motorConsMissedSteps[0]+=3;
Serial.print("R99");
Serial.print(" missed step ");
//Serial.print(motorConsMissedSteps[0]);
//Serial.print(" encoder pos ");
//Serial.print(encoderX.currentPosition());
//Serial.print(" axis pos ");
//Serial.print(axisX.currentPosition());
Serial.print("\n");
axisX.setCurrentPosition(encoderX.currentPosition());
} else {
if (motorConsMissedSteps[0] > 0) {
motorConsMissedSteps[0]--;
}
}
axisX.resetStepDone();
if (motorConsMissedSteps[0] > motorConsMissedStepsMax[0]) {
axisX.deactivateAxis();
Serial.print("R99");
Serial.print(" deactivate motor ");
Serial.print("\n");
}
}
if (axisX.endStopAxisReached(false)) {
axisX.setCurrentPosition(0);
encoderX.setPosition(0);
}
//encoderX.readEncoder();
//encoderX.test();
@ -176,6 +247,14 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
axisActive[1] = axisY.isAxisActive();
axisActive[2] = axisZ.isAxisActive();
currentPoint[0] = axisX.currentPosition();
currentPoint[1] = axisY.currentPosition();
currentPoint[2] = axisZ.currentPosition();
CurrentState::getInstance()->setX(currentPoint[0]);
CurrentState::getInstance()->setY(currentPoint[1]);
CurrentState::getInstance()->setZ(currentPoint[2]);
storeEndStops();
// Check timeouts
@ -199,7 +278,7 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
if (error == 1) {
Serial.print("R99 error\n");
Timer1.stop();
//Timer1.stop();
axisActive[0] = false;
axisActive[1] = false;
axisActive[2] = false;
@ -207,9 +286,21 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
// Periodically send message still active
currentMillis++;
if (currentMillis % 2500 == 0)
//if (currentMillis % 2500 == 0)
if (currentMillis % 750 == 0)
{
Serial.print("R04\n");
reportPosition();
//Serial.print("R99");
//Serial.print(" missed step nr ");
//Serial.print(motorConsMissedSteps[0]);
//Serial.print(" encoder pos ");
//Serial.print(encoderX.currentPosition());
//Serial.print(" axis pos ");
//Serial.print(axisX.currentPosition());
//Serial.print("\n");
}
}
@ -231,7 +322,7 @@ int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
reportEndStops();
reportPosition();
/**/encoderX.test();
/**///encoderX.test();
return error;
}
@ -469,7 +560,7 @@ int StepperControl::calibrateAxis(int axis) {
// Handle movement by checking each axis
void StepperControl::handleMovementInterrupt(void){
//encoderX.readEncoder();
encoderX.readEncoder();
//encoderY.readEncoder();
//encoderZ.readEncoder();
/**/
@ -541,7 +632,6 @@ void StepperControl::reportEndStops() {
void StepperControl::reportPosition(){
CurrentState::getInstance()->printPosition();
test2();
}
void StepperControl::storeEndStops() {

4
src/StepperControl.h
View File

@ -67,6 +67,10 @@ private:
bool motorInv[3];
bool endStInv[3];
long timeOut[3];
int motorConsMissedStepsMax[3];
int motorConsMissedSteps[3];
};
#endif /* STEPPERCONTROL_H_ */

94
src/StepperControlAxis.cpp
View File

@ -10,17 +10,17 @@ StepperControlAxis::StepperControlAxis() {
pinMax = 0;
axisActive = false;
consMissedSteps = 0;
motorConsMissedStepsMax = 10;
}
void StepperControlAxis::test() {
Serial.print("R99");
Serial.print(" cons steps missed ");
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(consMissedSteps);
Serial.print("\n");
}
@ -133,19 +133,26 @@ void StepperControlAxis::checkMovement() {
checkAxisDirection();
if (((coordCurrentPoint != coordDestinationPoint) || coordHomeAxis) && axisActive) {
// 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
// Get the axis speed, in steps per second
axisSpeed = calculateSpeed( coordSourcePoint, coordCurrentPoint, coordDestinationPoint,
motorSpeedMin, motorSpeedMax, motorStepsAcc);
// 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))) {
// If end stop reached or the encoder doesn't move anymore, don't move anymore
if ((consMissedSteps <= motorConsMissedStepsMax) && ((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)
{
@ -181,7 +188,6 @@ void StepperControlAxis::checkTiming() {
if (moveTicks >= stepOffTick) {
// Negative flank for the steps
verifyStepAxis();
resetMotorStep();
checkMovement();
}
@ -191,36 +197,35 @@ void StepperControlAxis::checkTiming() {
// Positive flank for the steps
setStepAxis();
movementStepDone = true;
}
}
}
}
void StepperControlAxis::verifyStepAxis() {
if (coordCurrentPoint != coordEncoderPoint) {
consMissedSteps++;
} else {
consMissedSteps = 0;
}
coordCurrentPoint = coordEncoderPoint;
}
void StepperControlAxis::setStepAxis() {
if (coordHomeAxis && coordCurrentPoint == 0) {
// if (coordHomeAxis && coordCurrentPoint == 0) {
// Keep moving toward end stop even when position is zero
// but end stop is not yet active
if (motorHomeIsUp) {
coordCurrentPoint = -1;
} else {
coordCurrentPoint = 1;
}
}
// if (motorHomeIsUp) {
// coordCurrentPoint = -1;
// } else {
// coordCurrentPoint = 1;
// }
// }
if (coordCurrentPoint < coordDestinationPoint) {
// if (coordCurrentPoint < coordDestinationPoint) {
// coordCurrentPoint++;
// } else if (coordCurrentPoint > coordDestinationPoint) {
// coordCurrentPoint--;
// }
if (movementUp) {
coordCurrentPoint++;
} else if (coordCurrentPoint > coordDestinationPoint) {
} else {
coordCurrentPoint--;
}
@ -228,10 +233,10 @@ void StepperControlAxis::setStepAxis() {
setMotorStep();
// if the home end stop is reached, set the current position
if (endStopAxisReached(false))
{
coordCurrentPoint = 0;
}
//if (endStopAxisReached(false))
//{
// coordCurrentPoint = 0;
//}
}
bool StepperControlAxis::endStopAxisReached(bool movement_forward) {
@ -370,6 +375,10 @@ bool StepperControlAxis::isAxisActive() {
return axisActive;
}
void StepperControlAxis::deactivateAxis() {
axisActive = false;
}
void StepperControlAxis::setMotorStep() {
digitalWrite(pinStep, HIGH);
}
@ -390,10 +399,6 @@ void StepperControlAxis::setCurrentPosition(long newPos) {
coordCurrentPoint = newPos;
}
void StepperControlAxis::setEncoderPosition(long newPos) {
coordEncoderPoint = newPos;
}
void StepperControlAxis::setMaxSpeed(long speed) {
motorSpeedMax = speed;
}
@ -401,3 +406,12 @@ void StepperControlAxis::setMaxSpeed(long speed) {
void StepperControlAxis::activateDebugPrint() {
debugPrint = true;
}
bool StepperControlAxis::isStepDone() {
return movementStepDone;
}
void StepperControlAxis::resetStepDone() {
movementStepDone = false;
}

11
src/StepperControlAxis.h
View File

@ -34,6 +34,8 @@ public:
void checkTiming();
bool isAxisActive();
void deactivateAxis();
bool endStopMin();
bool endStopMax();
bool endStopsReached();
@ -41,7 +43,6 @@ public:
long currentPosition();
void setCurrentPosition(long newPos);
void setEncoderPosition(long newPos);
void setMotorStep();
void resetMotorStep();
@ -52,6 +53,9 @@ public:
void setDirectionAway();
void setDirectionAxis();
bool isStepDone();
void resetStepDone();
void activateDebugPrint();
void test();
@ -78,14 +82,12 @@ private:
bool motorMotorInv; // invert motot direction
bool motorEndStopInv; // invert input (true/false) of end stops
long motorInterruptSpeed; // period of interrupt in micro seconds
int motorConsMissedStepsMax; // the number of consecutive missed steps before the axis is seen as stuck
// coordinates
long coordSourcePoint; // all coordinated in steps
long coordCurrentPoint;
long coordDestinationPoint;
bool coordHomeAxis; // homing command
long coordEncoderPoint;
// movement handling
unsigned long movementLength;
@ -97,10 +99,9 @@ private:
bool axisActive;
bool movementUp;
bool movementToHome;
int consMissedSteps;
bool movementStepDone;
void setStepAxis();
void verifyStepAxis();
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);

6
src/StepperControlEncoder.cpp
View File

@ -78,9 +78,9 @@ void StepperControlEncoder::readEncoder() {
position += delta;
if (delta != 0) {
test();
}
//if (delta != 0) {
// test();
//}
}