void stateMachine()
{
modeOld = mode;
button0 = parseUARTbuffer(joy);
switch (button0)
{
case ESTOP: // ESTOP motors
LOG("ESTOP stateMachine mode \n");
disableMotors();
//m_estop = 1;
break;
case CLEAR_ESTOP: // disable ESTOP
LOG("CLEAR_ESTOP stateMachine mode \n");
enableMotors();
//m_estop = 0;
break;
default:
//LOG("stateMachine default ... \n");
break;
}
}
void decodeMsgType()
{
Engines motors;
if (rcvmsgInfo[1] == TARGET_ADK)
{
switch(rcvmsgInfo[0])
{
case BLINKY_ON:
Serial.println("COMMAND: BLINKY_ON");
startBlinky();
break;
case BLINKY_OFF:
Serial.println("COMMAND: BLINKY_OFF");
stopBlinky();
break;
case MOTOR_CONTROL:
Serial.println("COMMAND: MOTOR_CONTROL");
motors = decodeEngines();
printRightMotorSignal(motors);
printLeftMotorSignal(motors);
rightMotorControl(motors.right);
leftMotorControl(motors.left);
break;
case MOTOR_STOP:
Serial.println("COMMAND: ENGINE_STOP");
setLeftPower(0);
setRightPower(0);
break;
case ENABLE_MOTORS:
Serial.println("COMMAND: ENABLE_ENGINES");
enableMotors();
break;
default:
Serial.println("COMMAND: Error, message is of unknown type. No action performed");
break;
}
}
}
/**
* 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;
}
/**
* 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;
}
void Mobot::forward(int speed) {
enableMotors();
leftMotor(0, speed);
rightMotor(0, speed);
}
void Mobot::spinRight(int speed) {
enableMotors();
leftMotor(0, speed);
rightMotor(1, speed);
}
void parseGcode(String commandLine) {
if (commandLine.equals("ota")) {
//server.enableWebSockets(false);
OtaUpdate();
return;
} else if (commandLine.equals("switch")) {
Switch();
return;
} else if (commandLine.equals("restart")) {
System.restart();
return;
} else if (commandLine.equals("pos")) {
reportStatus();
return;
} else if (commandLine.equals("enable")) {
enableMotors();
return;
} else if (commandLine.equals("disable")) {
disableMotors();
return;
} else if (commandLine.equals("stop")) {
for (int i = 0; i < 4; i++) {
nextPos[i] = curPos[i];
}
} else if (commandLine.startsWith("udpServerIP"))
{
Vector<String> commandToken;
int numToken = splitString(commandLine, ' ', commandToken);
if(numToken==2)
udpServerIP = commandToken[1];
if(numToken==3)
{
udpServerIP = commandToken[1];
udpServerPort = atoi(commandToken[2].c_str());
}
char buf[150];
sprintf(buf, "Udp server port %s, port: %d", udpServerIP.c_str(), udpServerPort);
String msgBack = String(buf);
sendToClients(msgBack);
return;
} else if (commandLine.startsWith("reassign")) {
//sendToClients(message)
//reassign x=3 y=0 z=2 e=1
Vector<String> commandToken;
int numToken = splitString(commandLine, ' ', commandToken);
for (int i = 1; i < numToken; i++) {
Vector<String> axisIndex;
String axisIndexStr = commandToken[i].c_str();
splitString(axisIndexStr, '=', axisIndex);
String axis = axisIndex[0].c_str();
if (axis.equals("x"))
x = atoi(axisIndex[1].c_str());
else if (axis.equals("y"))
y = atoi(axisIndex[1].c_str());
else if (axis.equals("z"))
z = atoi(axisIndex[1].c_str());
else if (axis.equals("e"))
e = atoi(axisIndex[1].c_str());
}
char buf[150];
sprintf(buf, "Reassign: x=%d y=%d z=%d e=%d\r\n", x, y, z, e);
String msgBack = String(buf);
sendToClients(msgBack);
return;
}
if (steppersOn) {
Vector<String> commandToken;
int numToken = splitString(commandLine, ' ', commandToken);
for (int i = 0; i < numToken; i++) {
Serial.printf("Command: %s\r\n", commandToken[i].c_str());
String motor = commandToken[i].substring(0, 1);
String sign = commandToken[i].substring(1, 2);
String posStr = "";
if (sign == "+" || sign == "-") {
posStr = commandToken[i].substring(2, commandToken[i].length());
} else {
sign = "";
posStr = commandToken[i].substring(1, commandToken[i].length());
}
int8_t index = -1;
if (motor == "X")
index = x;
else if (motor == "Y")
index = y;
else if (motor == "Z")
index = z;
else if (motor == "E")
index = e;
else if (motor == "T") {
deltat = atoi(posStr.c_str());
}
if (index > -1) {
if (sign == "+")
nextPos[index] = nextPos[index] + atol(posStr.c_str());
else if (sign == "-")
nextPos[index] = nextPos[index] - atol(posStr.c_str());
else
nextPos[index] = atol(posStr.c_str());
char buf[150];
sprintf(buf, "Set nextpos[%d] to %d\r\n", index, nextPos[index]);
Serial.printf(buf);
String msgBack = String(buf);
sendToClients(msgBack);
}
}
}
}
