Frontend to the famous GRBL.

autolevelproxy.cpp 8.4KB

    /* Part of x2grbl * * Copyright Johann Wilhelm <johann.wilhelm@9mal6.de> 2015 * * see Readme.md for detailed license and usage information! */ #include "autolevelproxy.h" #include <QDebug> #include <QSettings> AutoLevelProxy::AutoLevelProxy(QObject *parent) : GCodeOutputDevice(parent) { device=nullptr; gridData=nullptr; minXGrid=0; maxXGrid=0; minYGrid=0; maxYGrid=0; x=0; y=0; z=0; highZ=0; millingDepth=0; probingGrid=0; millingDepth=0.2; millingSpeed=100; rapidSpeed=500; changeG1toG0forRapid=false; rapidMotionForSeeking=false; numGridDataPoints=-1; } AutoLevelProxy::~AutoLevelProxy() { if (gridData) { delete gridData; gridData=nullptr; } } void AutoLevelProxy::setOutputDevice(GCodeOutputDevice &Device) { device=&Device; Device.Reset(); } bool AutoLevelProxy::supported(GCode &Cmd) { if (device) { return device->supported(Cmd); } else { return true; } } void AutoLevelProxy::Reset() { x=0; y=0; z=0; if (device) { QList<QVector3D> probingData=device->getProbingData(); for (int i=0;i<probingData.count() && i<numGridDataPoints;i++) { QVector3D data=probingData[i]; int indexX=data.x()/probingGrid - minXGrid; int indexY=data.y()/probingGrid - minYGrid; if (indexX<0 || indexY<0) { continue; } if (indexX>=(maxXGrid-minXGrid) || indexY>=maxYGrid-minYGrid) { continue; } gridData[indexX*(maxYGrid-minYGrid)+indexY]=data.z(); } } } bool AutoLevelProxy::Run(GCode &Cmd) { if (Cmd.Code==CodeType_GCode) { if (Cmd.Cmd==0 || Cmd.Cmd==1) { if (Cmd.Parameters.contains(QString("Z"))) { if (Cmd.Parameters.contains(QString("X")) || Cmd.Parameters.contains(QString("Y"))) { qDebug()<<Cmd.serialize()<<"\n"; qDebug()<<"error 3-axis movement!"<<"\n"; } } else { } double nextX=x; double nextY=y; double nextZ=z; if (Cmd.Parameters.contains("X")) { nextX=Cmd.Parameters["X"].toDouble(); } if (Cmd.Parameters.contains("Y")) { nextY=Cmd.Parameters["Y"].toDouble(); } if (Cmd.Parameters.contains("Z")) { nextZ=Cmd.Parameters["Z"].toDouble(); } double dX=nextX-x; double dY=nextY-y; double dist=sqrt( (dX*dX) + (dY*dY) ); if (dist>probingGrid) { qDebug()<<Cmd.serialize()<<"Needs to be split\n"; int splittingFactor=(int)(dist/probingGrid) + 1; for (int i=1;i<splittingFactor;i++) { double intermediateX=x+(dX*i)/(double)splittingFactor; double intermediateY=y+(dY*i)/(double)splittingFactor; GCode IntermediateCode=Cmd; if (IntermediateCode.Parameters.contains("X")) IntermediateCode.Parameters["X"]=QString("%1").arg(intermediateX); if (IntermediateCode.Parameters.contains("Y")) IntermediateCode.Parameters["Y"]=QString("%1").arg(intermediateY); if (nextZ<0) { double replacementZ=loopupHeight(intermediateX, intermediateY, nextZ, millingDepth); IntermediateCode.Parameters["Z"]=QString("%1").arg(replacementZ); } else { IntermediateCode.Parameters["Z"]=QString("%1").arg(highZ); } qDebug()<<"\tSending intermediate Code:"<<IntermediateCode.serialize()<<"\n"; if (device) { if (!device->Run(IntermediateCode)) { return false; } } } } if (nextZ<0) { double replacementZ=loopupHeight(nextX, nextY, nextZ, millingDepth); Cmd.Parameters["Z"]=QString("%1").arg(replacementZ); } else { Cmd.Parameters["Z"]=QString("%1").arg(highZ); //replace seeking... if (rapidMotionForSeeking) Cmd.Cmd=0; } x=nextX; y=nextY; z=nextZ; } else { qDebug()<<"Code "<<Cmd.serialize()<<" might not be supported jet!"<<"\n"; } } if (device) { return device->Run(Cmd); } return true; } double AutoLevelProxy::loopupHeight(double x, double y, double originalZ, double adjust) { int indexX=x/probingGrid - minXGrid; int indexY=y/probingGrid - minYGrid; if (indexX<0 || indexY<0) { return originalZ; } if (indexX>=(maxXGrid-minXGrid) || indexY>=maxYGrid-minYGrid) { return originalZ; } if (indexX*(maxYGrid-minYGrid)+indexY<numGridDataPoints) return gridData[indexX*(maxYGrid-minYGrid)+indexY]-adjust; else return originalZ; } QString AutoLevelProxy::name() { return QString("AutoLevelerProxy"); } void AutoLevelProxy::ProbeGrid(double minX, double maxX, double minY, double maxY) { device->Reset(); probingGrid=5; rapidMotionForSeeking=true; double feedrate=100; double moveFeedrate=1000; double highZ=0; double Zlow=-100; QSettings settings(QSettings::IniFormat, QSettings::UserScope, QString("2.5d Grbl-Commander")); settings.beginGroup("Probing"); if (settings.contains(QString("ProbingGrid"))) probingGrid=settings.value("ProbingGrid").toDouble(); if (settings.contains(QString("ProbingFeedrate"))) feedrate=settings.value("ProbingFeedrate").toDouble(); if (settings.contains(QString("MovingFeedrate"))) moveFeedrate=settings.value("MovingFeedrate").toDouble(); if (settings.contains(QString("Zhigh"))) highZ=settings.value("Zhigh").toDouble(); if (settings.contains(QString("Zlow"))) Zlow=settings.value("Zlow").toDouble(); if (settings.contains(QString("RapidMotionSeek"))) rapidMotionForSeeking=settings.value("RapidMotionSeek").toBool(); settings.endGroup(); millingDepth=0.2; millingSpeed=100; rapidSpeed=500; changeG1toG0forRapid=false; settings.beginGroup("Milling"); if (settings.contains("MillingDepth")) millingDepth=settings.value("MillingDepth").toDouble(); if (settings.contains("MillingSpeed")) millingSpeed=settings.value("MillingSpeed").toDouble(); if (settings.contains("RapidMotionSpeed")) rapidSpeed=settings.value("RapidMotionSpeed").toDouble(); if (settings.contains("ChangeNonMillingToRapid")) changeG1toG0forRapid=settings.value("ChangeNonMillingToRapid").toBool(); settings.endGroup(); GCode TmpCode; TmpCode.Code=CodeType_RAW; GCode ProbeCode; ProbeCode.Code=CodeType_GCode; ProbeCode.Cmd=38; ProbeCode.SubCmd=2; ProbeCode.Parameters["F"]=QString("%1").arg(feedrate); ProbeCode.Parameters["Z"]=QString("%1").arg(Zlow); GCode MoveCode; MoveCode.Code=CodeType_GCode; if (rapidMotionForSeeking) MoveCode.Cmd=0; else MoveCode.Cmd=1; MoveCode.Parameters["F"]=QString("%1").arg(moveFeedrate); MoveCode.Parameters["Z"]=QString("%1").arg(highZ); minXGrid=minX/probingGrid-1; maxXGrid=maxX/probingGrid+1; minYGrid=minY/probingGrid-1; maxYGrid=maxY/probingGrid+1; if (gridData) { delete gridData; gridData=nullptr; } numGridDataPoints=(maxXGrid-minXGrid) * (maxYGrid-minYGrid); gridData=new double[(maxXGrid-minXGrid) * (maxYGrid-minYGrid)]; for (int x=minXGrid;x<maxXGrid;x++) { for (int y=minYGrid;y<maxYGrid;y++) { gridData[x*(maxYGrid-minYGrid) + y]=std::numeric_limits<double>::quiet_NaN(); MoveCode.Parameters["X"]=QString("%1").arg(x*probingGrid); MoveCode.Parameters["Y"]=QString("%1").arg(y*probingGrid); device->Run(MoveCode); ProbeCode.Parameters["X"]=QString("%1").arg(x*probingGrid); ProbeCode.Parameters["Y"]=QString("%1").arg(y*probingGrid); //qDebug()<<MoveCode.serialize()<<"\n"; //qDebug()<<ProbeCode.serialize()<<"\n"; device->Run(ProbeCode); device->Run(MoveCode); } } }