334 lines
5.1 KiB
C++
334 lines
5.1 KiB
C++
#include "Command.h"
|
|
|
|
const char axisCodes[3] = {'X', 'Y', 'Z'};
|
|
const char axisSpeedCodes[3] = {'A', 'B', 'C'};
|
|
const char speedCode = 'S';
|
|
const char parameterIdCode = 'P';
|
|
const char parameterValueCode = 'V';
|
|
const char parameterValue2Code = 'W';
|
|
const char elementCode = 'E';
|
|
const char timeCode = 'T';
|
|
const char modeCode = 'M';
|
|
const char msgQueueCode = 'Q';
|
|
|
|
CommandCodeEnum commandCodeEnum = CODE_UNDEFINED;
|
|
|
|
Command::Command(char *commandChar)
|
|
{
|
|
|
|
char *charBuf = commandChar;
|
|
char *charPointer;
|
|
bool invalidCommand = false;
|
|
|
|
charPointer = strtok(charBuf, " \n\r");
|
|
|
|
if (charPointer[0] == 'G' || charPointer[0] == 'F')
|
|
{
|
|
commandCodeEnum = getGCodeEnum(charPointer);
|
|
}
|
|
else
|
|
{
|
|
invalidCommand = true;
|
|
return;
|
|
}
|
|
|
|
while (charPointer != NULL)
|
|
{
|
|
getParameter(charPointer);
|
|
|
|
charPointer = strtok(NULL, " \n\r");
|
|
}
|
|
}
|
|
|
|
CommandCodeEnum Command::getGCodeEnum(char *code)
|
|
{
|
|
|
|
if (strcmp(code, "G0") == 0 || strcmp(code, "G00") == 0)
|
|
{
|
|
return G00;
|
|
}
|
|
if (strcmp(code, "G1") == 0 || strcmp(code, "G01") == 0)
|
|
{
|
|
return G01;
|
|
}
|
|
//if (strcmp(code, "F3") == 0 || strcmp(code, "F03") == 0) {
|
|
// return F03;
|
|
//}
|
|
|
|
if (strcmp(code, "F11") == 0)
|
|
{
|
|
return F11;
|
|
}
|
|
if (strcmp(code, "F12") == 0)
|
|
{
|
|
return F12;
|
|
}
|
|
if (strcmp(code, "F13") == 0)
|
|
{
|
|
return F13;
|
|
}
|
|
if (strcmp(code, "F14") == 0)
|
|
{
|
|
return F14;
|
|
}
|
|
if (strcmp(code, "F15") == 0)
|
|
{
|
|
return F15;
|
|
}
|
|
if (strcmp(code, "F16") == 0)
|
|
{
|
|
return F16;
|
|
}
|
|
|
|
if (strcmp(code, "F20") == 0)
|
|
{
|
|
return F20;
|
|
}
|
|
if (strcmp(code, "F21") == 0)
|
|
{
|
|
return F21;
|
|
}
|
|
if (strcmp(code, "F22") == 0)
|
|
{
|
|
return F22;
|
|
}
|
|
|
|
if (strcmp(code, "F31") == 0)
|
|
{
|
|
return F31;
|
|
}
|
|
if (strcmp(code, "F32") == 0)
|
|
{
|
|
return F32;
|
|
}
|
|
|
|
if (strcmp(code, "F41") == 0)
|
|
{
|
|
return F41;
|
|
}
|
|
if (strcmp(code, "F42") == 0)
|
|
{
|
|
return F42;
|
|
}
|
|
if (strcmp(code, "F43") == 0)
|
|
{
|
|
return F43;
|
|
}
|
|
if (strcmp(code, "F44") == 0)
|
|
{
|
|
return F44;
|
|
}
|
|
|
|
if (strcmp(code, "F61") == 0)
|
|
{
|
|
return F61;
|
|
}
|
|
|
|
if (strcmp(code, "F81") == 0)
|
|
{
|
|
return F81;
|
|
}
|
|
if (strcmp(code, "F82") == 0)
|
|
{
|
|
return F82;
|
|
}
|
|
if (strcmp(code, "F83") == 0)
|
|
{
|
|
return F83;
|
|
}
|
|
if (strcmp(code, "F84") == 0)
|
|
{
|
|
return F84;
|
|
}
|
|
|
|
return CODE_UNDEFINED;
|
|
}
|
|
|
|
double minusNotAllowed(double value)
|
|
{
|
|
if (value < 0)
|
|
{
|
|
return 0;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
void Command::getParameter(char *charPointer)
|
|
{
|
|
|
|
//msgQueue = 24;
|
|
|
|
if (charPointer[0] == axisCodes[0])
|
|
{
|
|
axisValue[0] = atof(charPointer + 1);
|
|
//msgQueue = 77;
|
|
}
|
|
|
|
if (charPointer[0] == axisCodes[1])
|
|
{
|
|
axisValue[1] = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == axisCodes[2])
|
|
{
|
|
axisValue[2] = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == axisSpeedCodes[0])
|
|
{
|
|
axisSpeedValue[0] = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == axisSpeedCodes[1])
|
|
{
|
|
axisSpeedValue[1] = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == axisSpeedCodes[2])
|
|
{
|
|
axisSpeedValue[2] = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == speedCode)
|
|
{
|
|
speedValue = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == parameterIdCode)
|
|
{
|
|
parameterId = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == parameterValueCode)
|
|
{
|
|
parameterValue = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == parameterValue2Code)
|
|
{
|
|
parameterValue2 = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == elementCode)
|
|
{
|
|
element = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == timeCode)
|
|
{
|
|
time = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == modeCode)
|
|
{
|
|
mode = atof(charPointer + 1);
|
|
}
|
|
|
|
if (charPointer[0] == msgQueueCode)
|
|
{
|
|
msgQueue = atof(charPointer + 1);
|
|
//msgQueue = 5;
|
|
}
|
|
}
|
|
|
|
void Command::print()
|
|
{
|
|
Serial.print("R99 Command with code: ");
|
|
Serial.print(commandCodeEnum);
|
|
Serial.print(", X: ");
|
|
Serial.print(axisValue[0]);
|
|
Serial.print(", Y: ");
|
|
Serial.print(axisValue[1]);
|
|
Serial.print(", Z: ");
|
|
Serial.print(axisValue[2]);
|
|
Serial.print(", S: ");
|
|
Serial.print(speedValue);
|
|
Serial.print(", P: ");
|
|
Serial.print(parameterId);
|
|
Serial.print(", V: ");
|
|
Serial.print(parameterValue);
|
|
|
|
Serial.print(", W: ");
|
|
Serial.print(parameterValue2);
|
|
Serial.print(", T: ");
|
|
Serial.print(time);
|
|
Serial.print(", E: ");
|
|
Serial.print(element);
|
|
Serial.print(", M: ");
|
|
Serial.print(mode);
|
|
Serial.print(", Q: ");
|
|
Serial.print(msgQueue);
|
|
Serial.print("\r\n");
|
|
}
|
|
|
|
CommandCodeEnum Command::getCodeEnum()
|
|
{
|
|
return commandCodeEnum;
|
|
}
|
|
|
|
double Command::getX()
|
|
{
|
|
return axisValue[0];
|
|
}
|
|
|
|
double Command::getY()
|
|
{
|
|
return axisValue[1];
|
|
}
|
|
|
|
double Command::getZ()
|
|
{
|
|
return axisValue[2];
|
|
}
|
|
|
|
long Command::getA()
|
|
{
|
|
return axisSpeedValue[0];
|
|
}
|
|
|
|
long Command::getB()
|
|
{
|
|
return axisSpeedValue[1];
|
|
}
|
|
|
|
long Command::getC()
|
|
{
|
|
return axisSpeedValue[2];
|
|
}
|
|
|
|
long Command::getP()
|
|
{
|
|
return parameterId;
|
|
}
|
|
|
|
long Command::getV()
|
|
{
|
|
return parameterValue;
|
|
}
|
|
|
|
long Command::getW()
|
|
{
|
|
return parameterValue2;
|
|
}
|
|
|
|
long Command::getT()
|
|
{
|
|
return time;
|
|
}
|
|
|
|
long Command::getE()
|
|
{
|
|
return element;
|
|
}
|
|
|
|
long Command::getM()
|
|
{
|
|
return mode;
|
|
}
|
|
|
|
long Command::getQ()
|
|
{
|
|
//msgQueue = 9876;
|
|
return msgQueue;
|
|
}
|