private void HandleGCode(String cmd)
{
Debug.WriteLine(DateTime.Now.ToString() + " Handling G command: " + cmd);
switch (cmd)
{
case "G91": WriteOKResponse(cmd); return;
case "G90": WriteOKResponse(cmd); return;
case "G21": WriteOKResponse(cmd); return;
case "G20": WriteOKResponse(cmd); break;
}
if (cmd.StartsWith("G10"))
{
var parts = cmd.Split(' ');
foreach (var part in parts)
{
if (part.StartsWith("X"))
{
_work.X = _machine.X;
}
if (part.StartsWith("Y"))
{
_work.X = _machine.X;
}
if (part.StartsWith("Z"))
{
_work.X = _machine.X;
}
}
return;
}
var line = _parser.CleanupLine(cmd, idx);
var parsedLine = _parser.ParseMotionLine(line, idx);
if (parsedLine == null)
{
WriteOKResponse(parsedLine);
return;
}
if (parsedLine.Command == "G4")
{
System.Threading.Tasks.Task.Delay(250).Wait();
WriteOKResponse("G4");
}
if (parsedLine.Command == "G0" ||
parsedLine.Command == "G1" ||
parsedLine.Command == "G00" ||
parsedLine.Command == "G01")
{
Debug.WriteLine("Pausing For: " + parsedLine.EstimatedRunTime.ToString());
var finishTime = DateTime.Now + (_firmwareType == FirmwareTypes.LagoVista_PnP ? TimeSpan.FromSeconds(1) : parsedLine.EstimatedRunTime);
SpinWait.SpinUntil(() => DateTime.Now > finishTime);
var parts = cmd.Split(' ');
foreach (var part in parts.Where(itm => !String.IsNullOrEmpty(itm)))
{
switch (part.Substring(0, 1))
{
case "X":
_machine.X = GetParamValue(part);
break;
case "Y":
_machine.Y = GetParamValue(part);
break;
case "Z":
_machine.Z = GetParamValue(part);
break;
case "C":
_rotation = GetParamValue(part);
break;
}
}
WriteOKResponse(parsedLine);
}
else if (parsedLine.Command.StartsWith("G04"))
{
var finishTime = DateTime.Now + TimeSpan.FromSeconds(3);
Debug.WriteLine("Handing Pause until " + finishTime + " or " + 3 + " seconds");
SpinWait.SpinUntil(() => DateTime.Now > finishTime);
WriteOKResponse(parsedLine);
Debug.WriteLine("Done " + DateTime.Now);
}
else if (parsedLine.Command.StartsWith("G38"))
{
var finishTime = DateTime.Now + TimeSpan.FromSeconds(3);
SpinWait.SpinUntil(() => DateTime.Now > finishTime);
var newHeight = _rnd.NextDouble() - 0.5;
AddResponse($"[PRB:0.00,0.00,{newHeight}:1]");
WriteOKResponse(parsedLine);
}
else if (parsedLine.Command.StartsWith("G92"))
{
WriteOKResponse(parsedLine);
}
}