public GrblStatusResponce(string responce)
{
responce = responce.TrimStart('<').TrimEnd('>');
var fields = responce.Split('|');
var status = fields[0];
foreach (var field in fields.Skip(1))
{
var components = field.Split(':');
var header = components[0];
var content = components[1];
switch (header)
{
case "MPos":
var mPos = content.Split(',');
this.MachinePosition = new PositionVector(mPos[0], mPos[1], mPos[2]);
break;
case "Bf":
var bf = content.Split(',');
this.PlannerBlocksAvailble = int.Parse(bf[0]);
this.RxBufferBytesAvailble = int.Parse(bf[1]);
break;
case "FS":
var fs = content.Split(',');
this.FeedRate = int.Parse(fs[0]);
this.RPM = int.Parse(fs[1]);
break;
case "WCO":
var wco = content.Split(',');
this.WorkCoordinateOffset = new PositionVector(wco[0], wco[1], wco[2]);
break;
case "Pn":
// TODO: support pins
break;
default:
break;
}
}
}
private void StateMachine()
{
switch (state)
{
case ProcessState.Init:
this.parser.Open = true;
state = ProcessState.CheckIfOnline;
break;
case ProcessState.CheckIfOnline:
if (this.parser.Open)
{
state = ProcessState.CheckSettings;
Thread.Sleep(300);
}
else
{
state = ProcessState.Init;
Thread.Sleep(500);
}
break;
case ProcessState.CheckSettings:
var settings = new List <string>();
settings.Add("$0=10"); // (step pulse, usec)
settings.Add("$1=25"); // (step idle delay, msec)
settings.Add("$2=0"); // (step port invert mask:00000000)
settings.Add("$3=3"); // (dir port invert mask:00000011)
settings.Add("$4=0"); // (step enable invert, bool)
settings.Add("$5=0"); // (limit pins invert, bool)
settings.Add("$6=0"); // (probe pin invert, bool)
settings.Add("$10=3"); // (status report mask:00000011)
settings.Add("$11=0.020"); // (junction deviation, mm)
settings.Add("$12=0.001"); // (arc tolerance, mm)
settings.Add("$13=0"); //(report inches, bool)
settings.Add("$20=0"); // (soft limits, bool)
settings.Add("$21=0"); // (hard limits, bool)
settings.Add("$22=0"); // (homing cycle, bool)
settings.Add("$23=1"); // (homing dir invert mask:00000001)
settings.Add("$24=50.000"); // (homing feed, mm/min)
settings.Add("$25=635.000"); // (homing seek, mm/min)
settings.Add("$26=250"); // (homing debounce, msec)
settings.Add("$27=1.000"); // (homing pull-off, mm)
settings.Add("$100=320.000"); // (x, step/mm)
settings.Add("$101=320.000"); // (y, step/mm)
settings.Add("$102=320.000"); // (z, step/mm)
settings.Add("$110=3000.000"); // (x max rate, mm/min)
settings.Add("$111=3000.000"); // (y max rate, mm/min)
settings.Add("$112=3000.000"); // (z max rate, mm/min)
settings.Add("$120=50.000"); // (x accel, mm/sec^2)
settings.Add("$121=50.000"); // (y accel, mm/sec^2)
settings.Add("$122=50.000"); // (z accel, mm/sec^2)
settings.Add("$130=225.000"); // (x max travel, mm)
settings.Add("$131=125.000"); // (y max travel, mm)
settings.Add("$132=170.000"); // (z max travel, mm)
var result = this.parser.SendCommand("$$");
if (String.IsNullOrEmpty(result))
{
this.parser.Open = false;
state = ProcessState.Init;
Thread.Sleep(5000);
return;
}
var currentSettings = result.Split(new string[] { "\r\n", "\n" }, StringSplitOptions.None);
foreach (var setting in currentSettings)
{
settings.RemoveAll(s => s == setting);
}
foreach (var setting in settings)
{
QueueCommand(setting);
}
if (settings.Count > 0)
{
this.parser.SendCommand(((char)24).ToString());
}
state = ProcessState.Idle;
break;
case ProcessState.Idle:
if (statusCheck.Ready)
{
state = ProcessState.CheckStatus;
}
else
{
state = ProcessState.WriteOutput;
}
break;
case ProcessState.WriteOutput:
state = ProcessState.Idle;
while (this.PlannerBlocksAvailble > 0)
{
var nextCommand = queue.GetNextCommand();
// no more commands
if (String.IsNullOrEmpty(nextCommand))
{
break;
}
// grbl rxBuffer full
if (this.RxBufferBytesAvailble < (nextCommand.Length + 5))
{
break;
}
this.RxBufferBytesAvailble -= (nextCommand.Length + 2);
this.PlannerBlocksAvailble--;
var message = this.parser.SendCommand(nextCommand);
queue.Remove(nextCommand);
if (!String.IsNullOrEmpty(message) && message.Contains("[MSG:Pgm End]"))
{
this.JobActive = false;
}
}
break;
case ProcessState.CheckStatus:
state = ProcessState.Idle;
var status = this.parser.ReadStaus();
if (status == null)
{
return;
}
this.PlannerBlocksAvailble = status.PlannerBlocksAvailble;
this.RxBufferBytesAvailble = status.RxBufferBytesAvailble;
if (MachinePosition == null)
{
MachinePosition = status.MachinePosition;
WorkCoordinate = new PositionVector(MachinePosition.X, MachinePosition.Y, MachinePosition.Z);
}
else
{
MachinePosition.Move(status.MachinePosition);
WorkCoordinate.Move(MachinePosition);
}
if (status.WorkCoordinateOffset != null)
{
WorkCoordinate.Offset(status.WorkCoordinateOffset);
}
GrblWebSocket.SendToWebSocketClients(this.NamedParameters.ToJSON());
break;
}
}
public void Move(PositionVector newPosition)
{
this.X = newPosition.X;
this.Y = newPosition.Y;
this.Z = newPosition.Z;
}
public void Offset(PositionVector newOffset)
{
this.offsetX = -newOffset.X;
this.offsetY = -newOffset.Y;
this.offsetZ = -newOffset.Z;
}
