private void ManageCommandResponse(string rline)
{
try
{
if (mPending.Count > 0)
{
GrblCommand pending = mPending.Dequeue();
pending.SetResult(rline, SupportCSV);
mBuffer -= pending.SerialData.Length;
if (mTP.InProgram && pending.RepeatCount == 0) //solo se non è una ripetizione aggiorna il tempo
{
mTP.JobExecuted(pending.TimeOffset);
}
//ripeti errori programma && non ho una coda (magari mi sto allineando per cambio conf buff/sync) && ho un errore && non l'ho già ripetuto troppe volte
if (InProgram && CurrentStreamingMode == StreamingMode.RepeatOnError && mPending.Count == 0 && pending.Status == GrblCommand.CommandStatus.ResponseBad && pending.RepeatCount < 3) //il comando eseguito ha dato errore
{
mRetryQueue = new GrblCommand(pending.Command, pending.RepeatCount + 1); //repeat on error
}
}
if (InProgram && mQueuePtr.Count == 0 && mPending.Count == 0)
{
OnProgramEnd();
}
}
catch (Exception ex)
{
Logger.LogMessage("CommandResponse", "Ex on [{0}] message", rline);
Logger.LogException("CommandResponse", ex);
}
}
public G2G3Helper(StatePositionBuilder spb, GrblCommand cmd)
{
bool jb = cmd.JustBuilt;
if (!jb)
{
cmd.BuildHelper();
}
double aX = (double)spb.X.Previous; //startX
double aY = (double)spb.Y.Previous; //startY
double bX = (double)spb.X.Number; //endX
double bY = (double)spb.Y.Number; //endY
double oX = cmd.I != null ? (double)cmd.I.Number : 0.0; //offsetX
double oY = cmd.J != null ? (double)cmd.J.Number : 0.0; //offsetY
CenterX = aX + oX; //centerX
CenterY = aY + oY; //centerY
Ray = Math.Sqrt(oX * oX + oY * oY); //raggio
RectX = CenterX - Ray;
RectY = CenterY - Ray;
RectW = 2 * Ray;
RectH = 2 * Ray;
StartAngle = CalculateAngle(CenterX, CenterY, aX, aY); //angolo iniziale
EndAngle = CalculateAngle(CenterX, CenterY, bX, bY); //angolo finale
AngularWidth = AngularDistance(StartAngle, EndAngle, spb.G2);
if (!jb)
{
cmd.DeleteHelper();
}
}
private void SendLine()
{
GrblCommand tosend = PeekNextCommand();
if (tosend != null)
{
try
{
tosend.BuildHelper();
tosend.SetSending();
mSentPtr.Add(tosend);
mPending.Enqueue(tosend);
RemoveManagedCommand();
mBuffer += tosend.SerialData.Length;
com.Write(tosend.SerialData);
if (mTP.InProgram)
{
mTP.JobSent();
}
}
catch (Exception ex)
{
if (tosend != null)
{
Logger.LogMessage("SendLine", "Error sending [{0}] command", tosend.Command);
}
Logger.LogException("SendLine", ex);
}
finally { tosend.DeleteHelper(); }
}
}
public TimeSpan AnalyzeCommand(GrblCommand cmd, bool compute, GrblConf conf = null)
{
bool delete = !cmd.JustBuilt;
if (!cmd.JustBuilt)
{
cmd.BuildHelper();
}
UpdateModalsNB(cmd);
mCurX.Update(cmd.X, ABS);
mCurY.Update(cmd.Y, ABS);
mCurF.Update(cmd.F);
mCurS.Update(cmd.S);
TimeSpan rv = compute ? ComputeExecutionTime(cmd, conf) : TimeSpan.Zero;
if (delete)
{
cmd.DeleteHelper();
}
return(rv);
}
private void SendLine()
{
GrblCommand tosend = null;
try
{
tosend = mQueuePtr.Count > 0 ? mQueuePtr.Peek() : null;
if (tosend != null)
{
tosend.SetSending();
mSentPtr.Add(tosend);
mPending.Enqueue(tosend);
mQueuePtr.Dequeue();
mBuffer += (tosend.Command.Length + 2); //+2 for \r\n
com.WriteLine(tosend.Command);
if (mTP.InProgram)
{
mTP.JobSent();
}
}
}
catch (Exception ex)
{
if (tosend != null)
{
Logger.LogMessage("SendLine", "Error sending [{0}] command", tosend.Command);
}
Logger.LogException("SendLine", ex);
}
}
private void EmulateCommand(GrblCommand cmd)
{
if (!mCheck)
{
try
{
cmd.BuildHelper();
if (cmd.IsRelativeCoord)
{
abs = false;
}
if (cmd.IsAbsoluteCoord)
{
abs = true;
}
if (cmd.F != null)
{
speed = cmd.F.Number;
}
decimal newX = cmd.X != null ? (abs ? cmd.X.Number : curX + cmd.X.Number) : curX;
decimal newY = cmd.Y != null ? (abs ? cmd.Y.Number : curY + cmd.Y.Number) : curY;
decimal distance = 0;
if (cmd.IsLinearMovement)
{
distance = Tools.MathHelper.LinearDistance(curX, curY, newX, newY);
}
else if (cmd.IsArcMovement) //arc of given radius
{
distance = Tools.MathHelper.ArcDistance(curX, curY, newX, newY, cmd.GetArcRadius());
}
if (distance != 0 && speed != 0)
{
toSleep += TimeSpan.FromMinutes((double)distance / (double)speed);
}
if (toSleep.TotalMilliseconds > 15) //execute sleep
{
long start = Tools.HiResTimer.TotalNano;
System.Threading.Thread.Sleep(toSleep);
long stop = Tools.HiResTimer.TotalNano;
toSleep -= TimeSpan.FromMilliseconds((double)(stop - start) / 1000.0 / 1000.0);
}
curX = newX;
curY = newY;
}
catch (Exception ex) { throw ex; }
finally { cmd.DeleteHelper(); }
}
EnqueueTX("ok");
}
private void UpdateFS(GrblCommand cmd)
{
if (cmd is JogCommand)
{
return;
}
mCurF.Update(cmd.F);
mCurS.Update(cmd.S);
}
private void ClearQueue(bool sent)
{
mQueue.Clear();
mPending.Clear();
if (sent)
{
mSent.Clear();
}
mRetryQueue = null;
}
public void ExportConfig(string filename)
{
if (mMachineStatus == MacStatus.Idle)
{
try
{
using (System.IO.StreamWriter sw = new System.IO.StreamWriter(filename))
{
GrblCommand cmd = new GrblCommand("$$");
lock (this)
{
mSentPtr = new System.Collections.Generic.List <IGrblRow>(); //assign sent queue
mQueuePtr = new System.Collections.Generic.Queue <GrblCommand>();
mQueuePtr.Enqueue(cmd);
}
try
{
while (cmd.Result == null) //resta in attesa della risposta
{
;
}
if (cmd.Result != "OK")
{
System.Windows.Forms.MessageBox.Show("Error exporting config!", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error);
}
else
{
int msg = 0;
foreach (IGrblRow row in mSentPtr)
{
if (row is GrblMessage)
{
sw.WriteLine(((GrblMessage)row).Message);
msg++;
}
}
sw.Close();
System.Windows.Forms.MessageBox.Show(String.Format("{0} Config exported with success!", msg), "Success", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Information);
}
}catch { System.Windows.Forms.MessageBox.Show("Error exporting config!", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error); }
lock (this)
{
mQueuePtr = mQueue;
mSentPtr = mSent; //restore queue
}
}
}
catch { System.Windows.Forms.MessageBox.Show("Error exporting config!", "Error", System.Windows.Forms.MessageBoxButtons.OK, System.Windows.Forms.MessageBoxIcon.Error); }
}
}
private void RemoveManagedCommand()
{
if (mRetryQueue != null)
{
mRetryQueue = null;
}
else
{
mQueuePtr.Dequeue();
}
}
protected override void SendToSerial(GrblCommand tosend)
{
if (injob)
{
mUsedBuffer += tosend.SerialData.Length;
com.Write(tosend.SerialData); //invio dei dati alla linea di comunicazione
}
else
{
//mUsedBuffer += tosend.SerialData.Length;
com.Write(tosend.SerialData); //invio dei dati alla linea di comunicazione
tosend.SetResult("ok", false);
}
}
private decimal GetSegmentLenght(GrblCommand cmd)
{
if (cmd.IsLinearMovement)
{
return(Tools.MathHelper.LinearDistance(mCurX.Previous, mCurY.Previous, mCurX.Number, mCurY.Number));
}
else if (cmd.IsArcMovement) //arc of given radius
{
return(Tools.MathHelper.ArcDistance(mCurX.Previous, mCurY.Previous, mCurX.Number, mCurY.Number, cmd.GetArcRadius()));
}
else
{
return(0);
}
}
//private void UpdateModals(GrblCommand cmd) //update modals - BUILD IF NEEDED
//{
// bool delete = !cmd.JustBuilt;
// if (!cmd.JustBuilt) cmd.BuildHelper();
// UpdateModalsNB(cmd);
// if (delete) cmd.DeleteHelper();
//}
protected void UpdateModalsNB(GrblCommand cmd) //update modals - EXTERNAL BUILD
{
MotionMode.Update(cmd.G);
CoordinateSelect.Update(cmd.G);
PlaneSelect.Update(cmd.G);
DistanceMode.Update(cmd.G);
ArcDistanceMode.Update(cmd.G);
FeedRateMode.Update(cmd.G);
CutterRadiusCompensation.Update(cmd.G);
ToolLengthOffset.Update(cmd.G);
ProgramMode.Update(cmd.M);
CoolantState.Update(cmd.M);
SpindleState.Update(cmd.M);
}
private void UpdateXYZ(GrblCommand cmd)
{
if (cmd is JogCommand)
{
mCurX.Update(cmd.X, cmd.IsAbsoluteCoord, mWcoX);
mCurY.Update(cmd.Y, cmd.IsAbsoluteCoord, mWcoY);
mCurZ.Update(cmd.Z, cmd.IsAbsoluteCoord, mWcoZ);
}
else if (cmd.IsMovement)
{
mCurX.Update(cmd.X, ABS, mWcoX);
mCurY.Update(cmd.Y, ABS, mWcoY);
mCurZ.Update(cmd.Z, ABS, mWcoZ);
}
}
public virtual void Update(GrblCommand cmd)
{
bool delete = !cmd.JustBuilt;
if (!cmd.JustBuilt)
{
cmd.BuildHelper();
}
UpdateModals(cmd);
if (delete)
{
cmd.DeleteHelper();
}
}
private decimal GetSegmentLenght(GrblCommand cmd)
{
LastArcHelperResult = null;
if (cmd.IsLinearMovement)
{
return(Tools.MathHelper.LinearDistance(mCurX.Previous, mCurY.Previous, mCurX.Number, mCurY.Number));
}
else if (cmd.IsArcMovement) //arc of given radius
{
return((decimal)GetArcHelper(cmd).AbsLenght);
}
else
{
return(0);
}
}
protected void UpdateModals(GrblCommand cmd)
{
Element G = cmd.GetElement('G');
Element M = cmd.GetElement('M');
MotionMode.Update(G, M);
CoordinateSelect.Update(G, M);
PlaneSelect.Update(G, M);
DistanceMode.Update(G, M);
ArcDistanceMode.Update(G, M);
FeedRateMode.Update(G, M);
CutterRadiusCompensation.Update(G, M);
ToolLengthOffset.Update(G, M);
ProgramMode.Update(G, M);
SpindleState.Update(G, M);
CoolantState.Update(G, M);
}
private void UpdateWCO(GrblCommand cmd)
{
if (cmd.IsSetWCO)
{
if (cmd.X != null)
{
mWcoX = mCurX.Number - cmd.X.Number;
}
if (cmd.Y != null)
{
mWcoY = mCurY.Number - cmd.Y.Number;
}
if (cmd.Z != null)
{
mWcoZ = mCurZ.Number - cmd.Z.Number;
}
}
}
private TimeSpan ComputeExecutionTime(GrblCommand cmd, GrblConf conf)
{
if (G0 && cmd.IsLinearMovement)
{
return(TimeSpan.FromMinutes((double)GetSegmentLenght(cmd) / (double)conf.MaxRateX)); //todo: use a better computation of xy if different speed
}
else if (G1G2G3 && cmd.IsMovement && mCurF.Number != 0)
{
return(TimeSpan.FromMinutes((double)GetSegmentLenght(cmd) / (double)Math.Min(mCurF.Number, conf.MaxRateX)));
}
else if (cmd.IsPause)
{
return(cmd.P != null?TimeSpan.FromSeconds((double)cmd.P.Number) : cmd.S != null?TimeSpan.FromSeconds((double)cmd.S.Number) : TimeSpan.Zero);
}
else
{
return(TimeSpan.Zero);
}
}
public TimeSpan AnalyzeCommand(GrblCommand cmd, bool compute, GrblConf conf = null)
{
bool delete = !cmd.JustBuilt;
if (!cmd.JustBuilt)
{
cmd.BuildHelper();
}
UpdateModalsNB(cmd);
UpdateWCO(cmd);
UpdateXYZ(cmd);
UpdateFS(cmd);
TimeSpan rv = compute ? ComputeExecutionTime(cmd, conf) : TimeSpan.Zero;
if (delete)
{
cmd.DeleteHelper();
}
return(rv);
}
private void ManageCommandResponse(string rline)
{
try
{
if (mPending.Count > 0)
{
GrblCommand pending = mPending.Dequeue();
pending.SetResult(rline, SupportCSV);
mBuffer -= (pending.Command.Length + 2); //+2 for \r\n
if (mTP.InProgram)
{
mTP.JobExecuted(pending.TimeOffset);
}
}
}
catch (Exception ex)
{
Logger.LogMessage("CommandResponse", "Ex on [{0}] message", rline);
Logger.LogException("CommandResponse", ex);
}
}
public override void Update(GrblCommand cmd)
{
bool delete = !cmd.JustBuilt;
if (!cmd.JustBuilt)
{
cmd.BuildHelper();
}
UpdateModals(cmd);
if (cmd.TrueMovement(X.Number, Y.Number, Absolute))
{
if (cmd.X != null)
{
X.SetNumber(Absolute ? cmd.X.Number : X.Number + cmd.X.Number);
}
if (cmd.Y != null)
{
Y.SetNumber(Absolute ? cmd.Y.Number : Y.Number + cmd.Y.Number);
}
}
if (cmd.F != null)
{
F.SetNumber(cmd.F.Number);
}
if (cmd.S != null)
{
S.SetNumber(cmd.S.Number);
}
if (delete)
{
cmd.DeleteHelper();
}
}
private void SendLine()
{
try
{
GrblCommand tosend = mQueuePtr.Count > 0 ? mQueuePtr.Peek() : null;
if (tosend != null)
{
tosend.SetSending();
mSentPtr.Add(tosend);
mPending.Enqueue(tosend);
mQueuePtr.Dequeue();
mBuffer += (tosend.Command.Length + 2); //+2 for \r\n
com.WriteLine(tosend.Command);
if (mTP.InProgram)
{
mTP.JobSent();
}
}
}
catch {}
}
private bool HasSpaceInBuffer(GrblCommand command)
{
return((mBuffer + command.SerialData.Length) <= BUFFER_SIZE);
}
public GrblCommand(GrblElement first, GrblCommand toappend)
{
mLine = string.Format("{0} {1}", first, toappend.mLine);
}
private bool CanSend()
{
GrblCommand next = mQueuePtr.Count > 0 ? mQueuePtr.Peek() : null;
return(next != null && (mBuffer + next.Command.Length + 2) <= BUFFER_SIZE); //+2 for /r/n
}
public void EnqueueCommand(GrblCommand cmd)
{
lock (this)
{ mQueuePtr.Enqueue(cmd.Clone() as GrblCommand); }
}
internal G2G3Helper GetArcHelper(GrblCommand cmd)
{
LastArcHelperResult = new G2G3Helper(this, cmd);
return(LastArcHelperResult);
}
public GrblCommand(Element first, GrblCommand toappend)
{
mLine = string.Format("{0} {1}", first, toappend.mLine).ToUpper().Trim();
}
void ThreadRX()
{
try
{
string rline = null;
if ((rline = GetComLineOrDisconnect()) != null)
{
if (rline.Length > 0)
{
lock (this)
{
if (rline.ToLower().StartsWith("ok") || rline.ToLower().StartsWith("error"))
{
if (mPending.Count > 0)
{
GrblCommand pending = mPending.Dequeue();
//mLaserPosition = new System.Drawing.PointF(pending.X != null ? (float)pending.X.Number : mLaserPosition.X, pending.Y != null ? (float)pending.Y.Number : mLaserPosition.Y);
pending.SetResult(rline, SupportCSV);
mBuffer -= (pending.Command.Length + 2); //+2 for \r\n
if (mTP.InProgram)
{
mTP.JobExecuted(pending.TimeOffset);
}
//if (mQueue.Count == 0 && mPending.Count == 0)
//{
// if (QueueStatus != null)
// QueueStatus(true);
//}
}
}
else if (rline.StartsWith("<") && rline.EndsWith(">"))
{
rline = rline.Substring(1, rline.Length - 2);
System.Diagnostics.Debug.WriteLine(rline);
if (rline.Contains("|")) //grbl > 1.1
{
string[] arr = rline.Split("|".ToCharArray());
ParseMachineStatus(arr[0]);
string mpos = arr[1].Substring(5, arr[1].Length - 5);
string[] xyz = mpos.Split(",".ToCharArray());
mLaserPosition = new System.Drawing.PointF(float.Parse(xyz[0], System.Globalization.NumberFormatInfo.InvariantInfo), float.Parse(xyz[1], System.Globalization.NumberFormatInfo.InvariantInfo));
for (int i = 1; i < arr.Length; i++)
{
if (arr[i].StartsWith("Ov"))
{
ParseOverrides(arr[i]);
}
}
}
else //<Idle,MPos:0.000,0.000,0.000,WPos:0.000,0.000,0.000>
{
string[] arr = rline.Split(",".ToCharArray());
ParseMachineStatus(arr[0]);
mLaserPosition = new System.Drawing.PointF(float.Parse(arr[1].Substring(5, arr[1].Length - 5), System.Globalization.NumberFormatInfo.InvariantInfo), float.Parse(arr[2], System.Globalization.NumberFormatInfo.InvariantInfo));
}
}
else if (rline.StartsWith("Grbl "))
{
//Grbl vX.Xx ['$' for help]
try
{
int maj = int.Parse(rline.Substring(5, 1));
int min = int.Parse(rline.Substring(7, 1));
int build = (int)(rline.Substring(8, 1).ToCharArray()[0]);
mGrblVersion = new Version(maj, min, build);
}
catch {}
mSentPtr.Add(new GrblMessage(rline, false));
}
else
{
mSentPtr.Add(new GrblMessage(rline, SupportCSV));
}
}
}
}
} catch {}
}
