/**
* xDest - destination X in steps
* yDest - destination Y in steps
* zDest - destination Z in steps
* maxStepsPerSecond - maximum number of steps per second
* maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
*/
int StepperControl::moveToCoords( long xDest, long yDest, long zDest,
unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
bool xHome, bool yHome, bool zHome) {
unsigned long currentMillis = 0;
unsigned long timeStart = millis();
int incomingByte = 0;
int error = 0;
// load motor settings
loadMotorSettings();
// if a speed is given in the command, use that instead of the config speed
if (xMaxSpd > 0 && xMaxSpd < speedMax[0]) {
speedMax[0] = xMaxSpd;
}
if (yMaxSpd > 0 && yMaxSpd < speedMax[1]) {
speedMax[1] = yMaxSpd;
}
if (zMaxSpd > 0 && zMaxSpd < speedMax[2]) {
speedMax[2] = zMaxSpd;
}
axisX.setMaxSpeed(speedMax[0]);
axisY.setMaxSpeed(speedMax[1]);
axisZ.setMaxSpeed(speedMax[2]);
// Load coordinates into axis class
long sourcePoint[3] = {0,0,0};
sourcePoint[0] = CurrentState::getInstance()->getX();
sourcePoint[1] = CurrentState::getInstance()->getY();
sourcePoint[2] = CurrentState::getInstance()->getZ();
long currentPoint[3] = {0,0,0};
currentPoint[0] = CurrentState::getInstance()->getX();
currentPoint[1] = CurrentState::getInstance()->getY();
currentPoint[2] = CurrentState::getInstance()->getZ();
long destinationPoint[3]= {0,0,0};
destinationPoint[0] = xDest;
destinationPoint[1] = yDest;
destinationPoint[2] = zDest;
// Load coordinates into motor control
axisX.loadCoordinates(currentPoint[0],destinationPoint[0],xHome);
axisY.loadCoordinates(currentPoint[1],destinationPoint[1],yHome);
axisZ.loadCoordinates(currentPoint[2],destinationPoint[2],zHome);
// Prepare for movement
storeEndStops();
reportEndStops();
axisX.setDirectionAxis();
axisY.setDirectionAxis();
axisZ.setDirectionAxis();
enableMotors();
// Start movement
axisActive[0] = true;
axisActive[1] = true;
axisActive[2] = true;
axisX.checkMovement();
axisY.checkMovement();
axisZ.checkMovement();
Timer1.start();
// Let the interrupt handle all the movements
while (axisActive[0] || axisActive[1] || axisActive[2]) {
delay(1);
axisActive[0] = axisX.isAxisActive();
axisActive[1] = axisY.isAxisActive();
axisActive[2] = axisZ.isAxisActive();
storeEndStops();
// Check timeouts
if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000))) {
error = 1;
}
// Check if there is an emergency stop command
if (Serial.available() > 0) {
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101) {
error = 1;
}
}
if (error == 1) {
Serial.print("R99 error\n");
Timer1.stop();
axisActive[0] = false;
axisActive[1] = false;
axisActive[2] = false;
}
// Periodically send message still active
currentMillis++;
if (currentMillis % 2500 == 0)
{
Serial.print("R04\n");
}
}
Serial.print("R99 stopped\n");
Timer1.stop();
disableMotors();
currentPoint[0] = axisX.currentPoint();
currentPoint[1] = axisY.currentPoint();
currentPoint[2] = axisZ.currentPoint();
CurrentState::getInstance()->setX(currentPoint[0]);
CurrentState::getInstance()->setY(currentPoint[1]);
CurrentState::getInstance()->setZ(currentPoint[2]);
storeEndStops();
reportEndStops();
reportPosition();
return error;
}
int StepperControl::calibrateAxis(int axis)
{
// Load motor and encoder settings
loadMotorSettings();
loadEncoderSettings();
//unsigned long timeStart = millis();
bool movementDone = false;
int paramValueInt = 0;
int stepsCount = 0;
int incomingByte = 0;
int error = 0;
bool invertEndStops = false;
int parEndInv;
int parNbrStp;
float *missedSteps;
int *missedStepsMax;
long *lastPosition;
float *encoderStepDecay;
bool *encoderEnabled;
int *axisStatus;
long *axisStepsPerMm;
// Prepare for movement
storeEndStops();
reportEndStops();
// Select the right axis
StepperControlAxis *calibAxis;
StepperControlEncoder *calibEncoder;
switch (axis)
{
case 0:
calibAxis = &axisX;
calibEncoder = &encoderX;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_X;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_X;
invertEndStops = endStInv[0];
missedSteps = &motorConsMissedSteps[0];
missedStepsMax = &motorConsMissedStepsMax[0];
lastPosition = &motorLastPosition[0];
encoderStepDecay = &motorConsMissedStepsDecay[0];
encoderEnabled = &motorConsEncoderEnabled[0];
axisStatus = &axisSubStep[0];
axisStepsPerMm = &stepsPerMm[0];
break;
case 1:
calibAxis = &axisY;
calibEncoder = &encoderY;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y;
invertEndStops = endStInv[1];
missedSteps = &motorConsMissedSteps[1];
missedStepsMax = &motorConsMissedStepsMax[1];
lastPosition = &motorLastPosition[1];
encoderStepDecay = &motorConsMissedStepsDecay[1];
encoderEnabled = &motorConsEncoderEnabled[1];
axisStatus = &axisSubStep[1];
axisStepsPerMm = &stepsPerMm[1];
break;
case 2:
calibAxis = &axisZ;
calibEncoder = &encoderZ;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z;
invertEndStops = endStInv[2];
missedSteps = &motorConsMissedSteps[2];
missedStepsMax = &motorConsMissedStepsMax[2];
lastPosition = &motorLastPosition[2];
encoderStepDecay = &motorConsMissedStepsDecay[2];
encoderEnabled = &motorConsEncoderEnabled[2];
axisStatus = &axisSubStep[2];
axisStepsPerMm = &stepsPerMm[2];
break;
default:
Serial.print("R99 Calibration error: invalid axis selected\r\n");
error = 1;
CurrentState::getInstance()->setLastError(error);
return error;
}
// Preliminary checks
if (calibAxis->endStopMin() || calibAxis->endStopMax())
{
Serial.print("R99 Calibration error: end stop active before start\r\n");
error = 1;
CurrentState::getInstance()->setLastError(error);
return error;
}
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis->channelLabel);
Serial.print(" move to start for calibration");
Serial.print("\r\n");
*axisStatus = COMM_REPORT_MOVE_STATUS_START_MOTOR;
reportStatus(calibAxis, axisStatus[0]);
// Move towards home
calibAxis->enableMotor();
calibAxis->setDirectionHome();
calibAxis->setCurrentPosition(calibEncoder->currentPosition());
stepsCount = 0;
*missedSteps = 0;
movementDone = false;
motorConsMissedSteps[0] = 0;
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
*axisStatus = COMM_REPORT_MOVE_STATUS_CRAWLING;
reportStatus(calibAxis, axisStatus[0]);
reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_HOME);
while (!movementDone && error == 0)
{
#if defined(FARMDUINO_V14)
checkEncoders();
#endif
checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]);
// Check if there is an emergency stop command
if (Serial.available() > 0)
{
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101)
{
Serial.print("R99 emergency stop\r\n");
movementDone = true;
CurrentState::getInstance()->setEmergencyStop();
Serial.print(COMM_REPORT_EMERGENCY_STOP);
CurrentState::getInstance()->printQAndNewLine();
error = 1;
}
}
// Move until the end stop for home position is reached, either by end stop or motor skipping
if ((!calibAxis->endStopMin() && !calibAxis->endStopMax()) && !movementDone && (*missedSteps < *missedStepsMax))
{
calibAxis->setStepAxis();
delayMicroseconds(100000 / speedHome[axis] / 2);
stepsCount++;
if (stepsCount % (speedHome[axis] * 3) == 0)
{
// Periodically send message still active
Serial.print(COMM_REPORT_CMD_BUSY);
CurrentState::getInstance()->printQAndNewLine();
}
if (debugMessages)
{
if (stepsCount % (speedHome[axis] / 6) == 0 /*|| *missedSteps > 3*/)
{
Serial.print("R99");
Serial.print(" step count ");
Serial.print(stepsCount);
Serial.print(" missed steps ");
Serial.print(*missedSteps);
Serial.print(" max steps ");
Serial.print(*missedStepsMax);
Serial.print(" cur pos mtr ");
Serial.print(calibAxis->currentPosition());
Serial.print(" cur pos enc ");
Serial.print(calibEncoder->currentPosition());
Serial.print("\r\n");
}
}
calibAxis->resetMotorStep();
delayMicroseconds(100000 / speedHome[axis] / 2);
}
else
{
movementDone = true;
Serial.print("R99 movement done\r\n");
// If end stop for home is active, set the position to zero
if (calibAxis->endStopMax())
{
invertEndStops = true;
}
}
}
reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_TO_END);
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis->channelLabel);
Serial.print(" at starting point");
Serial.print("\r\n");
// Report back the end stop setting
if (error == 0)
{
if (invertEndStops)
{
paramValueInt = 1;
}
else
{
paramValueInt = 0;
}
Serial.print("R23");
Serial.print(" ");
Serial.print("P");
Serial.print(parEndInv);
Serial.print(" ");
Serial.print("V");
Serial.print(paramValueInt);
//Serial.print("\r\n");
CurrentState::getInstance()->printQAndNewLine();
}
// Store the status of the system
storeEndStops();
reportEndStops();
// Move into the other direction now, and measure the number of steps
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis->channelLabel);
Serial.print(" calibrating length");
Serial.print("\r\n");
stepsCount = 0;
movementDone = false;
*missedSteps = 0;
calibAxis->setDirectionAway();
calibAxis->setCurrentPosition(calibEncoder->currentPosition());
motorConsMissedSteps[0] = 0;
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
long encoderStartPoint = calibEncoder->currentPosition();
long encoderEndPoint = calibEncoder->currentPosition();
while (!movementDone && error == 0)
{
#if defined(FARMDUINO_V14)
checkEncoders();
#endif
checkAxisVsEncoder(calibAxis, calibEncoder, &motorConsMissedSteps[axis], &motorLastPosition[axis], &motorConsEncoderLastPosition[axis], &motorConsEncoderUseForPos[axis], &motorConsMissedStepsDecay[axis], &motorConsEncoderEnabled[axis]);
// Check if there is an emergency stop command
if (Serial.available() > 0)
{
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101)
{
Serial.print("R99 emergency stop\r\n");
movementDone = true;
CurrentState::getInstance()->setEmergencyStop();
Serial.print(COMM_REPORT_EMERGENCY_STOP);
CurrentState::getInstance()->printQAndNewLine();
error = 1;
}
}
// Ignore the missed steps at startup time
if (stepsCount < 10)
{
*missedSteps = 0;
}
// Move until the end stop at the other side of the axis is reached
if (((!invertEndStops && !calibAxis->endStopMax()) || (invertEndStops && !calibAxis->endStopMin())) && !movementDone && (*missedSteps < *missedStepsMax))
{
calibAxis->setStepAxis();
stepsCount++;
//checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
delayMicroseconds(100000 / speedHome[axis] / 2);
if (stepsCount % (speedHome[axis] * 3) == 0)
{
// Periodically send message still active
Serial.print(COMM_REPORT_CMD_BUSY);
//Serial.print("\r\n");
CurrentState::getInstance()->printQAndNewLine();
Serial.print("R99");
Serial.print(" step count: ");
Serial.print(stepsCount);
Serial.print("\r\n");
}
calibAxis->resetMotorStep();
delayMicroseconds(100000 / speedHome[axis] / 2);
}
else
{
Serial.print("R99 movement done\r\n");
movementDone = true;
}
}
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis->channelLabel);
Serial.print(" at end point");
Serial.print("\r\n");
encoderEndPoint = calibEncoder->currentPosition();
// if the encoder is enabled, use the encoder data instead of the step count
if (encoderEnabled)
{
stepsCount = abs(encoderEndPoint - encoderStartPoint);
}
// Report back the end stop setting
if (error == 0)
{
Serial.print("R23");
Serial.print(" ");
Serial.print("P");
Serial.print(parNbrStp);
Serial.print(" ");
Serial.print("V");
Serial.print((float)stepsCount);
CurrentState::getInstance()->printQAndNewLine();
}
*axisStatus = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
reportStatus(calibAxis, axisStatus[0]);
calibAxis->disableMotor();
storeEndStops();
reportEndStops();
switch (axis)
{
case 0:
CurrentState::getInstance()->setX(stepsCount);
break;
case 1:
CurrentState::getInstance()->setY(stepsCount);
break;
case 2:
CurrentState::getInstance()->setZ(stepsCount);
break;
}
reportPosition();
*axisStatus = COMM_REPORT_MOVE_STATUS_IDLE;
reportStatus(calibAxis, axisStatus[0]);
reportCalib(calibAxis, COMM_REPORT_CALIBRATE_STATUS_IDLE);
CurrentState::getInstance()->setLastError(error);
return error;
}
int StepperControl::calibrateAxis(int axis) {
// Load motor settings
loadMotorSettings();
//unsigned long timeStart = millis();
bool movementDone = false;
int paramValueInt = 0;
int stepsCount = 0;
int incomingByte = 0;
int error = 0;
bool invertEndStops = false;
int parEndInv;
int parNbrStp;
// Prepare for movement
storeEndStops();
reportEndStops();
// Select the right axis
StepperControlAxis calibAxis;
switch (axis) {
case 0:
calibAxis = axisX;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_X;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_X;
invertEndStops = endStInv[0];
break;
case 1:
calibAxis = axisY;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_Y;;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_Y;
invertEndStops = endStInv[0];
break;
case 2:
calibAxis = axisZ;
parEndInv = MOVEMENT_INVERT_ENDPOINTS_Z;
parNbrStp = MOVEMENT_AXIS_NR_STEPS_Z;
invertEndStops = endStInv[0];
break;
default:
Serial.print("R99 Calibration error: invalid axis selected\n");
return 1;
}
// Preliminary checks
if (calibAxis.endStopMin() || calibAxis.endStopMax()) {
Serial.print("R99 Calibration error: end stop active before start\n");
return 1;
}
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis.label);
Serial.print(" calibration start");
Serial.print("\n");
// Move towards home
calibAxis.enableMotor();
calibAxis.setDirectionHome();
stepsCount = 0;
movementDone = false;
while (!movementDone && error == 0) {
// Check if there is an emergency stop command
if (Serial.available() > 0) {
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101) {
movementDone = true;
error = 1;
}
}
// Move until the end stop for home position is reached
if ((!calibAxis.endStopMin() && !calibAxis.endStopMax()) && !movementDone) {
calibAxis.setMotorStep();
delayMicroseconds(1000000 / speedMin[axis] /2);
stepsCount++;
if (stepsCount % (speedMin[axis] * 3) == 0)
{
// Periodically send message still active
Serial.print("R04\n");
}
calibAxis.resetMotorStep();
delayMicroseconds(1000000 / speedMin[axis] /2);
} else {
movementDone = true;
// If end stop for home is active, set the position to zero
if (calibAxis.endStopMax())
{
invertEndStops = true;
}
}
}
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis.label);
Serial.print(" at first end stop");
Serial.print("\n");
// Report back the end stop setting
if (error == 0) {
if (invertEndStops) {
paramValueInt = 1;
} else {
paramValueInt = 0;
}
Serial.print("R23");
Serial.print(" ");
Serial.print("P");
Serial.print(parEndInv);
Serial.print(" ");
Serial.print("V");
Serial.print(paramValueInt);
Serial.print("\n");
}
// Store the status of the system
storeEndStops();
reportEndStops();
// Move into the other direction now, and measure the number of steps
stepsCount = 0;
movementDone = false;
calibAxis.setDirectionAway();
while (!movementDone && error == 0) {
// Check if there is an emergency stop command
if (Serial.available() > 0) {
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101) {
movementDone = true;
error = 1;
}
}
// Move until the end stop at the other side of the axis is reached
if (((!invertEndStops && !calibAxis.endStopMax()) || (invertEndStops && !calibAxis.endStopMin())) && !movementDone) {
calibAxis.setMotorStep();
stepsCount++;
delayMicroseconds(1000000 / speedMin[axis] /2);
if (stepsCount % (speedMin[axis] * 3) == 0)
{
// Periodically send message still active
Serial.print("R04\n");
}
calibAxis.resetMotorStep();
delayMicroseconds(1000000 / speedMin[axis] /2);
} else {
movementDone = true;
}
}
Serial.print("R99");
Serial.print(" axis ");
Serial.print(calibAxis.label);
Serial.print(" at second end stop");
Serial.print("\n");
// Report back the end stop setting
if (error == 0) {
Serial.print("R23");
Serial.print(" ");
Serial.print("P");
Serial.print(parNbrStp);
Serial.print(" ");
Serial.print("V");
Serial.print(stepsCount);
Serial.print("\n");
}
calibAxis.disableMotor();
storeEndStops();
reportEndStops();
switch (axis) {
case 0:
CurrentState::getInstance()->setX(stepsCount);
break;
case 1:
CurrentState::getInstance()->setY(stepsCount);
break;
case 2:
CurrentState::getInstance()->setZ(stepsCount);
break;
}
reportPosition();
return error;
}
/**
* xDest - destination X in steps
* yDest - destination Y in steps
* zDest - destination Z in steps
* maxStepsPerSecond - maximum number of steps per second
* maxAccelerationStepsPerSecond - maximum number of acceleration in steps per second
*/
int StepperControl::moveToCoords(double xDestScaled, double yDestScaled, double zDestScaled,
unsigned int xMaxSpd, unsigned int yMaxSpd, unsigned int zMaxSpd,
bool xHome, bool yHome, bool zHome)
{
long xDest = xDestScaled * stepsPerMm[0];
long yDest = yDestScaled * stepsPerMm[1];
long zDest = zDestScaled * stepsPerMm[2];
unsigned long currentMillis = 0;
unsigned long timeStart = millis();
serialMessageNr = 0;
serialMessageDelay = 0;
int incomingByte = 0;
int error = 0;
bool emergencyStop = false;
unsigned int commandSpeed[3];
// if a speed is given in the command, use that instead of the config speed
if (xMaxSpd > 0 && xMaxSpd < speedMax[0])
{
commandSpeed[0] = xMaxSpd;
}
else
{
commandSpeed[0] = speedMax[0];
}
if (yMaxSpd > 0 && yMaxSpd < speedMax[1])
{
commandSpeed[1] = yMaxSpd;
}
else
{
commandSpeed[1] = speedMax[1];
}
if (zMaxSpd > 0 && zMaxSpd < speedMax[2])
{
commandSpeed[2] = zMaxSpd;
}
else
{
commandSpeed[2] = speedMax[2];
}
axisX.setMaxSpeed(commandSpeed[0]);
axisY.setMaxSpeed(commandSpeed[1]);
axisZ.setMaxSpeed(commandSpeed[2]);
// Load coordinates into axis class
long sourcePoint[3] = {0, 0, 0};
sourcePoint[0] = CurrentState::getInstance()->getX();
sourcePoint[1] = CurrentState::getInstance()->getY();
sourcePoint[2] = CurrentState::getInstance()->getZ();
long currentPoint[3] = {0, 0, 0};
currentPoint[0] = CurrentState::getInstance()->getX();
currentPoint[1] = CurrentState::getInstance()->getY();
currentPoint[2] = CurrentState::getInstance()->getZ();
long destinationPoint[3] = {0, 0, 0};
destinationPoint[0] = xDest;
destinationPoint[1] = yDest;
destinationPoint[2] = zDest;
motorConsMissedSteps[0] = 0;
motorConsMissedSteps[1] = 0;
motorConsMissedSteps[2] = 0;
motorConsMissedStepsPrev[0] = 0;
motorConsMissedStepsPrev[1] = 0;
motorConsMissedStepsPrev[2] = 0;
motorLastPosition[0] = currentPoint[0];
motorLastPosition[1] = currentPoint[1];
motorLastPosition[2] = currentPoint[2];
// Load coordinates into motor control
// Report back coordinates if target coordinates changed
if (axisX.loadCoordinates(currentPoint[0], destinationPoint[0], xHome))
{
Serial.print(COMM_REPORT_COORD_CHANGED_X);
Serial.print(" X");
Serial.print(axisX.destinationPosition() / stepsPerMm[0]);
CurrentState::getInstance()->printQAndNewLine();
}
if (axisY.loadCoordinates(currentPoint[1], destinationPoint[1], yHome))
{
Serial.print(COMM_REPORT_COORD_CHANGED_Y);
Serial.print(" Y");
Serial.print(axisY.destinationPosition() / stepsPerMm[1]);
CurrentState::getInstance()->printQAndNewLine();
}
if (axisZ.loadCoordinates(currentPoint[2], destinationPoint[2], zHome))
{
Serial.print(COMM_REPORT_COORD_CHANGED_Z);
Serial.print(" Z");
Serial.print(axisZ.destinationPosition() / stepsPerMm[2]);
CurrentState::getInstance()->printQAndNewLine();
}
// Prepare for movement
axisX.movementStarted = false;
axisY.movementStarted = false;
axisZ.movementStarted = false;
storeEndStops();
reportEndStops();
axisX.setDirectionAxis();
axisY.setDirectionAxis();
axisZ.setDirectionAxis();
// Enable motors
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_START_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
enableMotors();
// Start movement
axisActive[0] = true;
axisActive[1] = true;
axisActive[2] = true;
if (xHome || yHome || zHome)
{
if (!xHome) { axisX.deactivateAxis(); }
if (!yHome) { axisY.deactivateAxis(); }
if (!zHome) { axisZ.deactivateAxis(); }
axisActive[0] = xHome;
axisActive[1] = yHome;
axisActive[2] = zHome;
}
axisX.checkMovement();
axisY.checkMovement();
axisZ.checkMovement();
axisX.setTicks();
axisY.setTicks();
axisZ.setTicks();
emergencyStop = CurrentState::getInstance()->isEmergencyStop();
// Let the interrupt handle all the movements
while ((axisActive[0] || axisActive[1] || axisActive[2]) && !emergencyStop)
{
#if defined(FARMDUINO_V14)
checkEncoders();
#endif
checkAxisSubStatus(&axisX, &axisSubStep[0]);
checkAxisSubStatus(&axisY, &axisSubStep[1]);
checkAxisSubStatus(&axisZ, &axisSubStep[2]);
axisX.checkTiming();
axisY.checkTiming();
axisZ.checkTiming();
if (axisX.isStepDone())
{
axisX.checkMovement();
checkAxisVsEncoder(&axisX, &encoderX, &motorConsMissedSteps[0], &motorLastPosition[0], &motorConsEncoderLastPosition[0], &motorConsEncoderUseForPos[0], &motorConsMissedStepsDecay[0], &motorConsEncoderEnabled[0]);
axisX.resetStepDone();
}
if (axisY.isStepDone())
{
axisY.checkMovement();
checkAxisVsEncoder(&axisY, &encoderY, &motorConsMissedSteps[1], &motorLastPosition[1], &motorConsEncoderLastPosition[1], &motorConsEncoderUseForPos[1], &motorConsMissedStepsDecay[1], &motorConsEncoderEnabled[1]);
axisY.resetStepDone();
}
if (axisZ.isStepDone())
{
axisZ.checkMovement();
checkAxisVsEncoder(&axisZ, &encoderZ, &motorConsMissedSteps[2], &motorLastPosition[2], &motorConsEncoderLastPosition[2], &motorConsEncoderUseForPos[2], &motorConsMissedStepsDecay[2], &motorConsEncoderEnabled[2]);
axisZ.resetStepDone();
}
if (axisX.isAxisActive() && motorConsMissedSteps[0] > motorConsMissedStepsMax[0])
{
axisX.deactivateAxis();
serialBuffer += "R99";
serialBuffer += " deactivate motor X due to missed steps";
serialBuffer += "\r\n";
if (xHome)
{
encoderX.setPosition(0);
axisX.setCurrentPosition(0);
}
else
{
error = 1;
}
}
if (axisY.isAxisActive() && motorConsMissedSteps[1] > motorConsMissedStepsMax[1])
{
axisY.deactivateAxis();
serialBuffer += "R99";
serialBuffer += " deactivate motor Y due to missed steps";
serialBuffer += "\r\n";
if (yHome)
{
encoderY.setPosition(0);
axisY.setCurrentPosition(0);
}
else
{
error = 1;
}
}
if (axisZ.isAxisActive() && motorConsMissedSteps[2] > motorConsMissedStepsMax[2])
{
axisZ.deactivateAxis();
serialBuffer += "R99";
serialBuffer += " deactivate motor Z due to missed steps";
serialBuffer += "\r\n";
if (zHome)
{
encoderZ.setPosition(0);
axisZ.setCurrentPosition(0);
}
else
{
error = 1;
}
}
if (axisX.endStopAxisReached(false))
{
axisX.setCurrentPosition(0);
encoderX.setPosition(0);
}
if (axisY.endStopAxisReached(false))
{
axisY.setCurrentPosition(0);
encoderY.setPosition(0);
}
if (axisZ.endStopAxisReached(false))
{
axisZ.setCurrentPosition(0);
encoderZ.setPosition(0);
}
axisActive[0] = axisX.isAxisActive();
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
if (axisActive[0] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[0] * 1000) || (millis() < timeStart && millis() > timeOut[0] * 1000)))
{
serialBuffer += COMM_REPORT_TIMEOUT_X;
serialBuffer += "\r\n";
serialBuffer += "R99 timeout X axis\r\n";
error = 2;
}
if (axisActive[1] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[1] * 1000) || (millis() < timeStart && millis() > timeOut[1] * 1000)))
{
serialBuffer += COMM_REPORT_TIMEOUT_Y;
serialBuffer += "\r\n";
serialBuffer += "R99 timeout Y axis\r\n";
error = 2;
}
if (axisActive[2] == true && ((millis() >= timeStart && millis() - timeStart > timeOut[2] * 1000) || (millis() < timeStart && millis() > timeOut[2] * 1000)))
{
serialBuffer += COMM_REPORT_TIMEOUT_Z;
serialBuffer += "\r\n";
serialBuffer += "R99 timeout Z axis\r\n";
error = 2;
}
// Check if there is an emergency stop command
if (Serial.available() > 0)
{
incomingByte = Serial.read();
if (incomingByte == 69 || incomingByte == 101)
{
serialBuffer += "R99 emergency stop\r\n";
Serial.print(COMM_REPORT_EMERGENCY_STOP);
CurrentState::getInstance()->printQAndNewLine();
emergencyStop = true;
axisX.deactivateAxis();
axisY.deactivateAxis();
axisZ.deactivateAxis();
axisActive[0] = false;
axisActive[1] = false;
axisActive[2] = false;
error = 1;
}
}
if (error != 0)
{
serialBuffer += "R99 error\r\n";
axisActive[0] = false;
axisActive[1] = false;
axisActive[2] = false;
}
// Send the serial buffer one character per cycle to keep motor timing more accuracte
serialBufferSendNext();
// Periodically send message still active
currentMillis++;
serialMessageDelay++;
if (serialMessageDelay > 300 && serialBuffer.length() == 0 && serialBufferSending == 0)
{
serialMessageDelay = 0;
switch(serialMessageNr)
{
case 0:
serialBuffer += COMM_REPORT_CMD_BUSY;
serialBuffer += CurrentState::getInstance()->getQAndNewLine();
break;
case 1:
serialBuffer += CurrentState::getInstance()->getPosition();
serialBuffer += CurrentState::getInstance()->getQAndNewLine();
break;
case 2:
#if defined(FARMDUINO_EXP_V20)
serialBuffer += "R89";
serialBuffer += " X";
serialBuffer += axisX.getLoad();
serialBuffer += " Y";
serialBuffer += axisY.getLoad();
serialBuffer += " Z";
serialBuffer += axisZ.getLoad();
serialBuffer += CurrentState::getInstance()->getQAndNewLine();
#endif
break;
}
serialMessageNr++;
#if !defined(FARMDUINO_EXP_V20)
if (serialMessageNr > 1)
{
serialMessageNr = 0;
}
#endif
#if defined(FARMDUINO_EXP_V20)
if (serialMessageNr > 2)
{
serialMessageNr = 0;
}
#endif
serialBufferSending = 0;
if (debugMessages /*&& debugInterrupt*/)
{
Serial.print("R99");
Serial.print(" missed step ");
Serial.print(motorConsMissedSteps[1]);
Serial.print(" encoder pos ");
Serial.print(encoderY.currentPosition());
Serial.print(" axis pos ");
Serial.print(axisY.currentPosition());
Serial.print("\r\n");
}
}
}
serialBufferEmpty();
Serial.print("R99 stopped\r\n");
// Send feedback for homing
if (xHome && !error && !emergencyStop)
{
Serial.print(COMM_REPORT_HOMED_X);
CurrentState::getInstance()->printQAndNewLine();
}
if (yHome && !error && !emergencyStop)
{
Serial.print(COMM_REPORT_HOMED_Y);
CurrentState::getInstance()->printQAndNewLine();
}
if (zHome && !error && !emergencyStop)
{
Serial.print(COMM_REPORT_HOMED_Z);
CurrentState::getInstance()->printQAndNewLine();
}
// Stop motors
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_STOP_MOTOR;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
disableMotors();
// Report end statuses
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();
reportEndStops();
reportPosition();
reportEncoders();
// Report axis idle
axisSubStep[0] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[1] = COMM_REPORT_MOVE_STATUS_IDLE;
axisSubStep[2] = COMM_REPORT_MOVE_STATUS_IDLE;
reportStatus(&axisX, axisSubStep[0]);
reportStatus(&axisY, axisSubStep[1]);
reportStatus(&axisZ, axisSubStep[2]);
disableMotors();
if (emergencyStop)
{
CurrentState::getInstance()->setEmergencyStop();
}
Serial.print("R99 error ");
Serial.print(error);
Serial.print("\r\n");
// Return error
CurrentState::getInstance()->setLastError(error);
return error;
}
