2014-05-15 11:44:28 -06:00
|
|
|
/*
|
|
|
|
* GCodeProcessor.cpp
|
|
|
|
*
|
|
|
|
* Created on: 15 maj 2014
|
|
|
|
* Author: MattLech
|
2014-11-04 15:14:35 -07:00
|
|
|
* Author: Tim Evers
|
2014-05-15 11:44:28 -06:00
|
|
|
*/
|
|
|
|
|
|
|
|
#include "GCodeProcessor.h"
|
2016-10-02 15:43:39 -06:00
|
|
|
#include "CurrentState.h"
|
2014-05-15 11:44:28 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
GCodeProcessor::GCodeProcessor()
|
|
|
|
{
|
2014-05-15 11:44:28 -06:00
|
|
|
}
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
GCodeProcessor::~GCodeProcessor()
|
|
|
|
{
|
2014-05-15 11:44:28 -06:00
|
|
|
}
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
void GCodeProcessor::printCommandLog(Command *command)
|
|
|
|
{
|
|
|
|
Serial.print("command == NULL: ");
|
|
|
|
Serial.println("\r\n");
|
2014-05-16 14:27:16 -06:00
|
|
|
}
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
int GCodeProcessor::execute(Command *command)
|
|
|
|
{
|
2016-12-03 13:58:27 -07:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
int execution = 0;
|
2017-07-08 12:17:16 -06:00
|
|
|
bool isMovement = false;
|
|
|
|
|
2018-01-05 19:00:36 -07:00
|
|
|
bool emergencyStop = false;
|
|
|
|
emergencyStop = CurrentState::getInstance()->isEmergencyStop();
|
|
|
|
|
2017-07-08 12:17:16 -06:00
|
|
|
int attempt = 0;
|
|
|
|
int maximumAttempts = ParameterList::getInstance()->getValue(PARAM_MOV_NR_RETRY);
|
2016-09-30 15:06:13 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
long Q = command->getQ();
|
|
|
|
CurrentState::getInstance()->setQ(Q);
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-07-08 12:17:16 -06:00
|
|
|
if
|
|
|
|
(
|
|
|
|
command->getCodeEnum() == G00 ||
|
|
|
|
command->getCodeEnum() == G01 ||
|
|
|
|
command->getCodeEnum() == F11 ||
|
|
|
|
command->getCodeEnum() == F12 ||
|
|
|
|
command->getCodeEnum() == F13 ||
|
|
|
|
command->getCodeEnum() == F14 ||
|
|
|
|
command->getCodeEnum() == F15 ||
|
|
|
|
command->getCodeEnum() == F16
|
|
|
|
)
|
|
|
|
{
|
|
|
|
isMovement = true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
2017-04-28 12:36:56 -06:00
|
|
|
//Only allow reset of emergency stop when emergency stop is engaged
|
|
|
|
|
2018-01-05 19:00:36 -07:00
|
|
|
if (emergencyStop)
|
2017-04-28 12:36:56 -06:00
|
|
|
{
|
|
|
|
if (!(
|
|
|
|
command->getCodeEnum() == F09 ||
|
|
|
|
command->getCodeEnum() == F20 ||
|
|
|
|
command->getCodeEnum() == F21 ||
|
|
|
|
command->getCodeEnum() == F22 ||
|
|
|
|
command->getCodeEnum() == F81 ||
|
|
|
|
command->getCodeEnum() == F82 ||
|
|
|
|
command->getCodeEnum() == F83 ))
|
|
|
|
{
|
|
|
|
|
|
|
|
Serial.print(COMM_REPORT_EMERGENCY_STOP);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
}
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-04-28 12:36:56 -06:00
|
|
|
// Do not execute the command when the config complete parameter is not
|
|
|
|
// set by the raspberry pi and it's asked to do a move command
|
|
|
|
|
2018-02-06 12:22:57 -07:00
|
|
|
if (ParameterList::getInstance()->getValue(PARAM_CONFIG_OK) != 1)
|
2017-04-28 12:36:56 -06:00
|
|
|
{
|
2018-02-06 12:22:57 -07:00
|
|
|
if (isMovement)
|
2017-04-28 12:36:56 -06:00
|
|
|
{
|
2018-02-06 12:22:57 -07:00
|
|
|
Serial.print(COMM_REPORT_NO_CONFIG);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
2019-10-16 14:41:06 -06:00
|
|
|
CurrentState::getInstance()->setLastError(ERR_PARAMS_NOT_OK);
|
2018-02-06 12:22:57 -07:00
|
|
|
return -1;
|
|
|
|
}
|
2017-04-28 12:36:56 -06:00
|
|
|
}
|
2017-04-15 15:32:01 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
// Return error when no command or invalid command is found
|
|
|
|
|
|
|
|
if (command == NULL)
|
|
|
|
{
|
2017-07-27 14:00:11 -06:00
|
|
|
|
|
|
|
Serial.print(COMM_REPORT_BAD_CMD);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
2019-10-16 14:41:06 -06:00
|
|
|
CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
|
2017-07-27 14:00:11 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
if (LOGGING)
|
|
|
|
{
|
|
|
|
printCommandLog(command);
|
|
|
|
}
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (command->getCodeEnum() == CODE_UNDEFINED)
|
|
|
|
{
|
2017-07-27 14:00:11 -06:00
|
|
|
Serial.print(COMM_REPORT_BAD_CMD);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
2019-10-16 14:41:06 -06:00
|
|
|
CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
|
2017-07-27 14:00:11 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
if (LOGGING)
|
|
|
|
{
|
|
|
|
printCommandLog(command);
|
|
|
|
}
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Get the right handler for this command
|
|
|
|
|
|
|
|
GCodeHandler *handler = getGCodeHandler(command->getCodeEnum());
|
|
|
|
|
|
|
|
if (handler == NULL)
|
|
|
|
{
|
2017-07-27 14:00:11 -06:00
|
|
|
Serial.print(COMM_REPORT_BAD_CMD);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
2019-10-16 14:41:06 -06:00
|
|
|
CurrentState::getInstance()->setLastError(ERR_INVALID_COMMAND);
|
2017-07-27 14:00:11 -06:00
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
Serial.println("R99 handler == NULL\r\n");
|
|
|
|
return -1;
|
|
|
|
}
|
|
|
|
|
2017-07-08 12:17:16 -06:00
|
|
|
// Report start of command
|
2017-04-19 08:12:12 -06:00
|
|
|
|
|
|
|
Serial.print(COMM_REPORT_CMD_START);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
|
|
|
|
2017-07-08 12:17:16 -06:00
|
|
|
// Execute command with retry
|
|
|
|
CurrentState::getInstance()->setLastError(0);
|
2018-01-05 19:20:01 -07:00
|
|
|
while (attempt < 1 || (!emergencyStop && attempt < maximumAttempts && execution != 0 && execution != 2))
|
|
|
|
// error 2 is timeout error: stop movement retries
|
2017-07-08 12:17:16 -06:00
|
|
|
{
|
2017-04-23 12:50:33 -06:00
|
|
|
|
2017-07-27 14:00:11 -06:00
|
|
|
if (LOGGING || debugMessages)
|
|
|
|
{
|
|
|
|
Serial.print("R99 attempt ");
|
|
|
|
Serial.print(attempt);
|
|
|
|
Serial.print(" from ");
|
|
|
|
Serial.print(maximumAttempts);
|
|
|
|
Serial.print("\r\n");
|
|
|
|
}
|
2017-07-08 12:17:16 -06:00
|
|
|
|
|
|
|
attempt++;
|
|
|
|
if (attempt > 1)
|
|
|
|
{
|
|
|
|
Serial.print(COMM_REPORT_CMD_RETRY);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
|
|
|
}
|
|
|
|
|
|
|
|
handler->execute(command);
|
|
|
|
execution = CurrentState::getInstance()->getLastError();
|
2018-01-05 19:00:36 -07:00
|
|
|
emergencyStop = CurrentState::getInstance()->isEmergencyStop();
|
2017-07-08 12:17:16 -06:00
|
|
|
|
2017-07-27 14:00:11 -06:00
|
|
|
if (LOGGING || debugMessages)
|
|
|
|
{
|
|
|
|
Serial.print("R99 execution ");
|
|
|
|
Serial.print(execution);
|
|
|
|
Serial.print("\r\n");
|
|
|
|
}
|
2017-07-08 12:17:16 -06:00
|
|
|
}
|
2017-07-27 14:00:11 -06:00
|
|
|
|
2017-04-23 12:50:33 -06:00
|
|
|
// Clean serial buffer
|
|
|
|
while (Serial.available() > 0)
|
|
|
|
{
|
|
|
|
Serial.read();
|
|
|
|
}
|
|
|
|
|
2017-07-27 14:00:11 -06:00
|
|
|
// if movemement failed after retry
|
|
|
|
// and parameter for emergency stop is set
|
|
|
|
// set the emergency stop
|
2017-07-30 07:16:35 -06:00
|
|
|
|
|
|
|
if (execution != 0)
|
2017-07-27 14:00:11 -06:00
|
|
|
{
|
|
|
|
if (isMovement)
|
|
|
|
{
|
|
|
|
if (ParameterList::getInstance()->getValue(PARAM_E_STOP_ON_MOV_ERR) == 1)
|
|
|
|
{
|
|
|
|
CurrentState::getInstance()->setEmergencyStop();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2017-04-23 12:50:33 -06:00
|
|
|
// Report back result of execution
|
2017-04-19 08:12:12 -06:00
|
|
|
if (execution == 0)
|
|
|
|
{
|
|
|
|
Serial.print(COMM_REPORT_CMD_DONE);
|
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
|
|
|
}
|
|
|
|
else
|
|
|
|
{
|
|
|
|
Serial.print(COMM_REPORT_CMD_ERROR);
|
2019-10-16 14:41:06 -06:00
|
|
|
Serial.print(" V");
|
|
|
|
Serial.print(CurrentState::getInstance()->getLastError());
|
2017-04-19 08:12:12 -06:00
|
|
|
CurrentState::getInstance()->printQAndNewLine();
|
|
|
|
}
|
|
|
|
|
|
|
|
CurrentState::getInstance()->resetQ();
|
|
|
|
return execution;
|
2014-05-15 11:44:28 -06:00
|
|
|
};
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
GCodeHandler *GCodeProcessor::getGCodeHandler(CommandCodeEnum codeEnum)
|
|
|
|
{
|
|
|
|
|
|
|
|
GCodeHandler *handler = NULL;
|
|
|
|
|
|
|
|
// These are if statements so they can be disabled as test
|
|
|
|
// Usefull when running into memory issues again
|
|
|
|
|
|
|
|
if (codeEnum == G00)
|
|
|
|
{
|
|
|
|
handler = G00Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (codeEnum == G28)
|
|
|
|
{
|
|
|
|
handler = G28Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
2017-04-26 14:45:05 -06:00
|
|
|
if (codeEnum == F09)
|
|
|
|
{
|
|
|
|
handler = F09Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
2017-04-19 08:12:12 -06:00
|
|
|
if (codeEnum == F11)
|
|
|
|
{
|
|
|
|
handler = F11Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F12)
|
|
|
|
{
|
|
|
|
handler = F12Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F13)
|
|
|
|
{
|
|
|
|
handler = F13Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (codeEnum == F14)
|
|
|
|
{
|
|
|
|
handler = F14Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F15)
|
|
|
|
{
|
|
|
|
handler = F15Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F16)
|
|
|
|
{
|
|
|
|
handler = F16Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (codeEnum == F20)
|
|
|
|
{
|
|
|
|
handler = F20Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F21)
|
|
|
|
{
|
|
|
|
handler = F21Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F22)
|
|
|
|
{
|
|
|
|
handler = F22Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
// if (codeEnum == F31) {handler = F31Handler::getInstance();}
|
|
|
|
// if (codeEnum == F32) {handler = F32Handler::getInstance();}
|
|
|
|
|
|
|
|
if (codeEnum == F41)
|
|
|
|
{
|
|
|
|
handler = F41Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F42)
|
|
|
|
{
|
|
|
|
handler = F42Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F43)
|
|
|
|
{
|
|
|
|
handler = F43Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F44)
|
|
|
|
{
|
|
|
|
handler = F44Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (codeEnum == F61)
|
|
|
|
{
|
|
|
|
handler = F61Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (codeEnum == F81)
|
|
|
|
{
|
|
|
|
handler = F81Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F82)
|
|
|
|
{
|
|
|
|
handler = F82Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F83)
|
|
|
|
{
|
|
|
|
handler = F83Handler::getInstance();
|
|
|
|
}
|
|
|
|
if (codeEnum == F84)
|
|
|
|
{
|
|
|
|
handler = F84Handler::getInstance();
|
|
|
|
}
|
|
|
|
|
|
|
|
return handler;
|
2014-05-15 11:44:28 -06:00
|
|
|
}
|