545 lines
12 KiB
C++
545 lines
12 KiB
C++
#include "gcodetovhf.h"
|
|
|
|
#include <QList>
|
|
#include <cassert>
|
|
#include <cmath>
|
|
#include <QVector2D>
|
|
#include <QVector3D>
|
|
#include <QDebug>
|
|
|
|
static constexpr double TOLLERANCE = 0.005;
|
|
|
|
QByteArray removeComments(const QByteArray& gcodeCommand)
|
|
{
|
|
size_t comment = 0;
|
|
QByteArray output;
|
|
for(size_t i = 0; i < gcodeCommand.size(); ++i)
|
|
{
|
|
if(gcodeCommand[i] == '(')
|
|
++comment;
|
|
else if(gcodeCommand[i] == ')' && comment > 0)
|
|
--comment;
|
|
|
|
if(comment == 0)
|
|
output.push_back(gcodeCommand[i]);
|
|
}
|
|
return output;
|
|
}
|
|
|
|
class Gmove
|
|
{
|
|
public:
|
|
int gmode = 0;
|
|
int coordmode = 54;
|
|
bool relative = false;
|
|
double xStart = 0;
|
|
double yStart = 0;
|
|
double zStart = 0;
|
|
double aStart = 0;
|
|
|
|
bool x = false;
|
|
bool y = false;
|
|
bool z = false;
|
|
bool a = false;
|
|
bool i = false;
|
|
bool j = false;
|
|
bool p = false;
|
|
bool r = false;
|
|
bool l = false;
|
|
|
|
double xVal;
|
|
double yVal;
|
|
double zVal;
|
|
double aVal;
|
|
double iVal;
|
|
double jVal;
|
|
double pVal;
|
|
double rVal;
|
|
double lVal;
|
|
|
|
void fill()
|
|
{
|
|
if(!x)
|
|
xVal = relative ? 0 : xStart;
|
|
if(!y)
|
|
yVal = relative ? 0 : yStart;
|
|
if(!z)
|
|
zVal = relative ? 0 : zStart;
|
|
if(!a)
|
|
aVal = relative ? 0 : aStart;
|
|
|
|
x = true;
|
|
y = true;
|
|
z = true;
|
|
}
|
|
void makeAbsolute()
|
|
{
|
|
if(!relative)
|
|
return;
|
|
if(x)
|
|
xVal = xVal+xStart;
|
|
if(y)
|
|
yVal = yVal+yStart;
|
|
if(z)
|
|
zVal = zVal+zStart;
|
|
if(a)
|
|
aVal = aVal+aStart;
|
|
relative = false;
|
|
}
|
|
};
|
|
|
|
static QString checkMove(Gmove gmove)
|
|
{
|
|
if(gmove.gmode == 0 || gmove.gmode == 1)
|
|
{
|
|
if(!gmove.x && !gmove.y && !gmove.z)
|
|
return "At least one coordinate is required for linear move.";
|
|
else if(gmove.i || gmove.j || gmove.p || gmove.r)
|
|
return "Invalid parameter in linear move.";
|
|
}
|
|
else if(gmove.gmode == 2 || gmove.gmode == 3)
|
|
{
|
|
if(!gmove.x && !gmove.y && !gmove.z)
|
|
return "At least one coordinate is required for arc move.";
|
|
else if(!gmove.i && !gmove.j && !gmove.r)
|
|
return "No arc center or radius defined.";
|
|
else if((gmove.i || gmove.j) && gmove.r)
|
|
return "More than one arc center defined.";
|
|
|
|
gmove.fill();
|
|
gmove.makeAbsolute();
|
|
QVector2D xyVect(gmove.xVal, gmove.yVal);
|
|
QVector2D startVect(gmove.xStart, gmove.yStart);
|
|
|
|
if(gmove.r)
|
|
{
|
|
if(gmove.rVal < TOLLERANCE)
|
|
return "Arc radius must be larger than zero.";
|
|
if((xyVect-startVect).length()/2 > gmove.rVal+TOLLERANCE)
|
|
return "Arc radius to small to fit to provided sart and end points.";
|
|
}
|
|
else
|
|
{
|
|
/*QVector2D ijVect(gmove.i ? gmove.iVal : 0, gmove.j ? gmove.jVal : 0);
|
|
QVector2D center = startVect+ijVect;
|
|
double r = (center-xyVect).length();
|
|
double r2 = (center-startVect).length();
|
|
|
|
if(abs(r-r2) > TOLLERANCE)
|
|
return "Arc start and end point radii differ";*/
|
|
}
|
|
}
|
|
|
|
return QString();
|
|
}
|
|
|
|
static QByteArray generateLinearCommand(const Gmove& gmove)
|
|
{
|
|
assert(gmove.gmode == 0 || gmove.gmode == 1);
|
|
|
|
QByteArray output;
|
|
if(gmove.gmode == 0)
|
|
{
|
|
if(gmove.relative)
|
|
output.append("GR");
|
|
else if(gmove.coordmode != 53)
|
|
output.append("GA");
|
|
else
|
|
output.append("GB");
|
|
}
|
|
else if(gmove.gmode == 1)
|
|
{
|
|
if(gmove.relative)
|
|
output.append("PR");
|
|
else if(gmove.coordmode != 53)
|
|
output.append("PA");
|
|
else
|
|
output.append("PB");
|
|
}
|
|
else
|
|
{
|
|
return output;
|
|
}
|
|
|
|
if(gmove.x)
|
|
output.append(QByteArray::number(static_cast<int>(gmove.xVal*1000)));
|
|
if(gmove.y || gmove.z || gmove.a)
|
|
output.append(',');
|
|
if(gmove.y)
|
|
output.append(QByteArray::number(static_cast<int>(gmove.yVal*1000)));
|
|
if(gmove.z || gmove.a)
|
|
output.append(',');
|
|
if(gmove.z)
|
|
output.append(QByteArray::number(static_cast<int>(gmove.zVal*-1000)));
|
|
if(gmove.a)
|
|
{
|
|
output.append(',');
|
|
output.append(QByteArray::number(static_cast<int>(gmove.aVal*1000)));
|
|
}
|
|
|
|
output.append(';');
|
|
|
|
return output;
|
|
}
|
|
|
|
static bool findCircleCenter(QVector2D a, QVector2D b, double r, bool leftHanded, QVector2D& result)
|
|
{ float distance = (a-b).length();
|
|
if(distance/2 > r+TOLLERANCE)
|
|
return false;
|
|
|
|
QVector2D baseVect = (b-a).normalized();
|
|
float loftDist = sqrt(pow(r, 2)-pow(distance/2, 2));
|
|
QVector2D loftVect;
|
|
if(leftHanded)
|
|
{
|
|
loftVect.setX(baseVect.y()*-1);
|
|
loftVect.setY(baseVect.x());
|
|
}
|
|
else
|
|
{
|
|
loftVect.setX(baseVect.y());
|
|
loftVect.setY(baseVect.x()*-1);
|
|
}
|
|
loftVect = loftVect*loftDist;
|
|
baseVect = baseVect*(distance/2);
|
|
result = a+baseVect+loftVect;
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool withinTollerance(double a, double b)
|
|
{
|
|
return a+TOLLERANCE > b && a-TOLLERANCE < b;
|
|
}
|
|
|
|
static QByteArray generateArcCommand(Gmove gmove, size_t resolution)
|
|
{
|
|
assert(gmove.gmode == 2 || gmove.gmode == 3);
|
|
gmove.fill();
|
|
gmove.makeAbsolute();
|
|
|
|
QVector2D startMove(gmove.xStart, gmove.yStart);
|
|
QVector2D endMove(gmove.xVal, gmove.yVal);
|
|
QVector2D center;
|
|
double radius;
|
|
if(gmove.i && gmove.j)
|
|
{
|
|
center.setX(gmove.xStart+gmove.iVal);
|
|
center.setY(gmove.yStart+gmove.jVal);
|
|
radius = (QVector2D(gmove.xStart, gmove.yStart)-center).length();
|
|
}
|
|
else if(gmove.r)
|
|
{
|
|
findCircleCenter(QVector2D(gmove.xStart, gmove.yStart), QVector2D(gmove.xVal, gmove.yVal), gmove.rVal, gmove.gmode == 3, center);
|
|
radius = gmove.rVal;
|
|
}
|
|
else
|
|
{
|
|
qCritical()<<"Can't create arc move";
|
|
return QByteArray();
|
|
}
|
|
|
|
QVector2D aVector = startMove-center;
|
|
QVector2D bVector = endMove-center;
|
|
double startAngle = acos(aVector.x()/(aVector.length()));
|
|
double endAngle = acos(bVector.x()/(bVector.length()));
|
|
|
|
{
|
|
bool a = false;
|
|
bool b = false;
|
|
QVector2D computedEnpoint(cos(endAngle)*radius+center.x(), sin(endAngle)*radius+center.y());
|
|
if((computedEnpoint-endMove).length() > TOLLERANCE)
|
|
endAngle = 0 - endAngle;
|
|
|
|
QVector2D computedSart(cos(startAngle)*radius+center.x(), sin(startAngle)*radius+center.y());
|
|
if((computedSart-startMove).length() > TOLLERANCE)
|
|
startAngle = 0 - startAngle;
|
|
|
|
if((startAngle-endAngle) < 0)
|
|
startAngle = startAngle+2*M_PI;
|
|
}
|
|
{
|
|
/*QVector2D computedEnpoint(cos(endAngle)*radius+center.x(), sin(endAngle)*radius+center.y());
|
|
assert((computedEnpoint-endMove).length() > TOLLERANCE);
|
|
|
|
QVector2D computedSart(cos(startAngle)*radius+center.x(), sin(startAngle)*radius+center.y());
|
|
assert((computedSart-startMove).length() > TOLLERANCE);*/
|
|
}
|
|
|
|
|
|
if(gmove.p && gmove.pVal > 1)
|
|
endAngle = endAngle+std::copysign(1.0, endAngle-startAngle)*2*(gmove.pVal-1)*M_PI;
|
|
|
|
size_t steps = (resolution*(fabs(endAngle-startAngle)))/(2*M_PI);
|
|
|
|
double angleStep;
|
|
if(gmove.gmode == 2)
|
|
angleStep = (endAngle-startAngle)/steps;
|
|
else
|
|
angleStep = (2*M_PI-(startAngle-endAngle))/steps;
|
|
|
|
QByteArray output;
|
|
double deltaZ = (gmove.zVal - gmove.zStart)/steps;
|
|
for(size_t i = 1; i < steps; ++i)
|
|
{
|
|
double workAngle = startAngle+angleStep*i;
|
|
QVector2D point;
|
|
point.setX(cos(workAngle)*radius);
|
|
point.setY(sin(workAngle)*radius);
|
|
point = point+center;
|
|
QByteArray command("PA");
|
|
command.append(QByteArray::number(static_cast<int>(point.x()*1000)) + "," +
|
|
QByteArray::number(static_cast<int>(point.y()*1000)) + "," +
|
|
QByteArray::number(static_cast<int>((gmove.zStart+deltaZ*i)*-1000)) + ";");
|
|
output.append(command);
|
|
}
|
|
|
|
QByteArray command("PA");
|
|
command.append(QByteArray::number(static_cast<int>(gmove.xVal*1000)) +"," +
|
|
QByteArray::number(static_cast<int>(gmove.yVal*1000)) + "," +
|
|
QByteArray::number(static_cast<int>(gmove.zVal*-1000)) + ";");
|
|
output.append(command);
|
|
|
|
return output;
|
|
}
|
|
|
|
static QByteArray generateDrillingCycle(Gmove gmove)
|
|
{
|
|
if(!gmove.r)
|
|
return QByteArray();
|
|
if(!gmove.z)
|
|
return QByteArray();
|
|
gmove.gmode = 0;
|
|
if(gmove.relative)
|
|
gmove.rVal = gmove.rVal+gmove.zVal;
|
|
gmove.makeAbsolute();
|
|
|
|
QByteArray output("GA,,"+QByteArray::number(static_cast<int>(gmove.rVal*-1000))+"; ");
|
|
if(gmove.x || gmove.y)
|
|
{
|
|
gmove.z = false;
|
|
output.append(generateLinearCommand(gmove));
|
|
}
|
|
|
|
size_t count = gmove.l ? gmove.lVal : 1;
|
|
|
|
for(size_t i = 0; i < count; ++i)
|
|
{
|
|
output.append("PA,,"+QByteArray::number(static_cast<int>(gmove.zVal*-1000))+"; ");
|
|
output.append("GA,,"+QByteArray::number(static_cast<int>(gmove.rVal*-1000))+"; ");
|
|
}
|
|
return output;
|
|
}
|
|
|
|
static QByteArray generateMoveCommand(const Gmove& gmove)
|
|
{
|
|
if(gmove.gmode == 0 || gmove.gmode == 1)
|
|
return generateLinearCommand(gmove);
|
|
else if(gmove.gmode == 2 || gmove.gmode == 3)
|
|
return generateArcCommand(gmove, 100);
|
|
else if(gmove.gmode == 81)
|
|
return generateDrillingCycle(gmove);
|
|
return QByteArray();
|
|
}
|
|
|
|
static Gmove doMove(Gmove gmove)
|
|
{
|
|
gmove.fill();
|
|
gmove.makeAbsolute();
|
|
|
|
Gmove out;
|
|
out.xStart = gmove.xVal;
|
|
out.yStart = gmove.yVal;
|
|
out.zStart = gmove.zVal;
|
|
out.aStart = gmove.aVal;
|
|
|
|
return out;
|
|
}
|
|
|
|
QByteArray gcodeToVhf(const QByteArray& gcode, bool* ok, QList<QString>* errors)
|
|
{
|
|
int gmode = -1;
|
|
int coordmode = 54;
|
|
bool relmode = false;
|
|
double spinspeed = 1000;
|
|
double feedrate = std::numeric_limits<double>::min();
|
|
int tool = 0;
|
|
Gmove move;
|
|
bool clearMove;
|
|
|
|
QByteArray output;
|
|
|
|
if(gcode.size()< 2)
|
|
return output;
|
|
|
|
QList<QByteArray> gcodeCommands = gcode.toUpper().split('\n');
|
|
|
|
if(ok)
|
|
*ok = true;
|
|
|
|
size_t line = 0;
|
|
for(QByteArray& command : gcodeCommands)
|
|
{
|
|
++line;
|
|
move.gmode = gmode;
|
|
move.coordmode = coordmode;
|
|
move.relative = relmode;
|
|
bool startSpindle = false;
|
|
bool toolchange = false;
|
|
command = removeComments(command);
|
|
|
|
QList<QByteArray> subcommands = command.split(' ');
|
|
for(QByteArray& subcommand : subcommands)
|
|
{
|
|
subcommand = subcommand.trimmed();
|
|
if(subcommand.size() < 1)
|
|
continue;
|
|
if(subcommand[0] == 'G')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
int gcodeCode = subcommand.toInt();
|
|
if(gcodeCode >= 0 && gcodeCode <= 3 || gcodeCode == 81)
|
|
gmode = gcodeCode;
|
|
else if(gcodeCode == 80 && gmode == 81)
|
|
gmode = 0;
|
|
else if(gcodeCode == 91)
|
|
relmode = true;
|
|
else if(gcodeCode == 90)
|
|
relmode = false;
|
|
else if(gcodeCode == 53)
|
|
move.coordmode = 53;
|
|
else if(gcodeCode > 54 && gcodeCode < 59)
|
|
move.coordmode = gcodeCode;
|
|
else if(gcodeCode == 28)
|
|
output.append("PB0,0,0,0; ");
|
|
else if(gcodeCode == 64)
|
|
clearMove = true;
|
|
|
|
move.gmode = gmode;
|
|
if(move.coordmode != 53)
|
|
move.coordmode = coordmode;
|
|
move.relative = relmode;
|
|
}
|
|
else if(subcommand[0] == 'M')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
int mCode = subcommand.toDouble();
|
|
switch(mCode)
|
|
{
|
|
case 0:
|
|
case 1:
|
|
output.append("!S; ");
|
|
break;
|
|
case 3:
|
|
startSpindle = true;
|
|
break;
|
|
case 2:
|
|
case 5:
|
|
output.append("RVS0; ");
|
|
startSpindle = false;
|
|
break;
|
|
case 6:
|
|
toolchange = true;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
else if(subcommand[0] == 'F')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
double newFeedrate = subcommand.toDouble();
|
|
if(abs(newFeedrate - feedrate) > TOLLERANCE)
|
|
{
|
|
feedrate = newFeedrate;
|
|
output.append("VS" + QByteArray::number(static_cast<int>(feedrate*(1000.0/60.0))) + "; ");
|
|
}
|
|
}
|
|
else if(subcommand[0] == 'T')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
tool = subcommand.toInt();
|
|
}
|
|
else if(subcommand[0] == 'S')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
spinspeed = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'X')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.x = true;
|
|
move.xVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'Y')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.y = true;
|
|
move.yVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'Z')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.z = true;
|
|
move.zVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'P')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.p = true;
|
|
move.pVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'A')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.a = true;
|
|
move.aVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'I')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.i = true;
|
|
move.iVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'J')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.j = true;
|
|
move.jVal = subcommand.toDouble();
|
|
}
|
|
else if(subcommand[0] == 'R')
|
|
{
|
|
subcommand.remove(0, 1);
|
|
move.r = true;
|
|
move.rVal = subcommand.toDouble();
|
|
}
|
|
}
|
|
|
|
if(toolchange)
|
|
output.append("T" + QByteArray::number(static_cast<int>(tool)) + "; ");
|
|
if(startSpindle)
|
|
output.append("RVS" + QByteArray::number(static_cast<int>(spinspeed)) + "; ");
|
|
if(move.x || move.y || move.z || move.a)
|
|
{
|
|
QString error = checkMove(move);
|
|
if(error.isEmpty())
|
|
{
|
|
output.append(generateMoveCommand(move));
|
|
}
|
|
else
|
|
{
|
|
*ok = false;
|
|
if(errors)
|
|
errors->push_back("Error on line " + QString::number(line) + ": " + error);
|
|
qWarning()<<"Error on line"<<line<<error;
|
|
}
|
|
move = doMove(move);
|
|
}
|
|
else if(clearMove)
|
|
move = doMove(move);
|
|
if(output.size() > 0 && output.back() != '\n')
|
|
output.push_back('\n');
|
|
}
|
|
|
|
return output;
|
|
}
|