public void Add(double[] values, AxisFlags axisFlags)
{
foreach (int i in axisFlags.ToIndices())
{
switch (i)
{
case 0:
X += values[0];
break;
case 1:
Y += values[1];
break;
case 2:
Z += values[2];
break;
case 3:
A += values[3];
break;
case 4:
B += values[4];
break;
case 5:
C += values[5];
break;
}
}
}
public void Set(double[] values, AxisFlags axisFlags, bool relative = false)
{
if (relative)
{
Add(values, axisFlags);
}
else
{
foreach (int i in axisFlags.ToIndices())
{
switch (i)
{
case 0:
X = values[0];
break;
case 1:
Y = values[1];
break;
case 2:
Z = values[2];
break;
case 3:
A = values[3];
break;
case 4:
B = values[4];
break;
case 5:
C = values[5];
break;
}
}
}
}
private void Probing_PropertyChanged(object sender, System.ComponentModel.PropertyChangedEventArgs e)
{
switch (e.PropertyName)
{
case nameof(ProbingViewModel.IsCompleted):
var probing = DataContext as ProbingViewModel;
probing.PropertyChanged -= Probing_PropertyChanged;
if (probing.IsSuccess)
{
bool ok = true;
int p = 0;
Position pos = new Position(probing.Grbl.MachinePosition);
foreach (int i in axisflags.ToIndices())
{
pos.Values[i] = probing.Positions[p++].Values[i] + (i == GrblConstants.Z_AXIS ? 0d : probing.ProbeDiameter / 2d * af[i]);
}
Position pz = new Position(pos);
pos.Z += probing.Offset.Z; // Sometimes NaN...?
if (probing.ProbeZ && axisflags != AxisFlags.Z)
{
ok = probing.GotoMachinePosition(pos, AxisFlags.Z);
pz.X += probing.ProbeDiameter / 2d * af[GrblConstants.X_AXIS];
pz.Y += probing.ProbeDiameter / 2d * af[GrblConstants.Y_AXIS];
if ((ok = (ok && probing.GotoMachinePosition(pz, axisflags))))
{
if ((ok = probing.WaitForResponse(string.Format(Probing.Command + "{0}F{1}", probing.Distance.ToString(AxisFlags.Z, true), probing.ProbeFeedRate.ToInvariantString()))))
{
axisflags |= AxisFlags.Z;
pos.Z = probing.Grbl.ProbePosition.Z;
pz.Z = pos.Z + probing.Offset.Z;
ok = probing.GotoMachinePosition(pz, AxisFlags.Z);
}
}
}
else
{
ok = probing.GotoMachinePosition(pos, AxisFlags.Z);
if (axisflags.HasFlag(AxisFlags.Z))
{
pos.Z = probing.Positions[0].Z;
pz.Z += probing.Offset.Z;
}
}
if (ok && probing.CoordinateMode == ProbingViewModel.CoordMode.G92)
{
if ((ok == probing.GotoMachinePosition(pos, axisflags)))
{
pos.X = pos.Y = pos.Z = 0d;
probing.Grbl.ExecuteCommand("G92" + pos.ToString(axisflags));
if (axisflags.HasFlag(AxisFlags.Z))
{
probing.GotoMachinePosition(pz, AxisFlags.Z);
}
}
}
else
{
probing.Grbl.ExecuteCommand(string.Format("G10L2P{0}{1}", probing.CoordinateSystem, pos.ToString(axisflags)));
}
probing.End(ok ? "Probing completed" : "Probing failed");
}
probing.Grbl.IsJobRunning = false;
break;
}
}
private void OnCompleted()
{
bool ok;
var probing = DataContext as ProbingViewModel;
if ((ok = probing.IsSuccess && probing.Positions.Count > 0))
{
int p = 0;
Position pos = new Position(probing.Grbl.MachinePosition);
foreach (int i in axisflags.ToIndices())
{
pos.Values[i] = probing.Positions[p++].Values[i] + (i == GrblConstants.Z_AXIS ? 0d : probing.ProbeDiameter / 2d * af[i]);
}
Position pz = new Position(pos);
pos.Z += probing.Depth; // Sometimes NaN...?
if (double.IsNaN(pos.Z))
{
probing.Grbl.IsJobRunning = false;
probing.Program.End("Probing failed, machine position not known");
return;
}
if (probing.ProbeZ && axisflags != AxisFlags.Z)
{
ok = probing.GotoMachinePosition(pos, AxisFlags.Z);
pz.X += probing.ProbeDiameter / 2d * af[GrblConstants.X_AXIS];
pz.Y += probing.ProbeDiameter / 2d * af[GrblConstants.Y_AXIS];
if ((ok = (ok && probing.GotoMachinePosition(pz, axisflags))))
{
ok = probing.WaitForResponse(probing.FastProbe + "Z-" + probing.Depth.ToInvariantString());
ok = ok && probing.WaitForResponse(probing.RapidCommand + "Z" + probing.LatchDistance.ToInvariantString());
ok = ok && probing.RemoveLastPosition();
if ((ok = ok && probing.WaitForResponse(probing.SlowProbe + "Z-" + probing.Depth.ToInvariantString())))
{
axisflags |= AxisFlags.Z;
pos.Z = probing.Grbl.ProbePosition.Z;
pz.Z = pos.Z + probing.Depth;
ok = probing.GotoMachinePosition(pz, AxisFlags.Z);
}
}
}
else
{
ok = probing.GotoMachinePosition(pos, AxisFlags.Z);
if (axisflags.HasFlag(AxisFlags.Z))
{
pos.Z = probing.Positions[0].Z;
pz.Z += probing.Depth;
}
}
if (ok)
{
if (probing.CoordinateMode == ProbingViewModel.CoordMode.G92)
{
if ((ok = probing.GotoMachinePosition(pos, axisflags)))
{
pos.X = pos.Y = 0d;
pos.Z = probing.WorkpieceHeight + probing.TouchPlateHeight;
probing.Grbl.ExecuteCommand("G92" + pos.ToString(axisflags));
if (axisflags.HasFlag(AxisFlags.Z))
{
probing.GotoMachinePosition(pz, AxisFlags.Z);
}
}
}
else
{
pos.Z -= probing.WorkpieceHeight + probing.TouchPlateHeight + probing.Grbl.ToolOffset.Z;
probing.Grbl.ExecuteCommand(string.Format("G10L2P{0}{1}", probing.CoordinateSystem, pos.ToString(axisflags)));
}
}
}
probing.Grbl.IsJobRunning = false;
probing.Program.End(ok ? "Probing completed" : "Probing failed");
}
private void OnCompleted()
{
bool ok;
var probing = DataContext as ProbingViewModel;
if ((ok = probing.IsSuccess && probing.Positions.Count > 0))
{
int p = 0;
Position pos = new Position(probing.StartPosition);
foreach (int i in axisflags.ToIndices())
{
pos.Values[i] = probing.Positions[p++].Values[i] + (i == GrblConstants.Z_AXIS ? 0d : probing.ProbeDiameter / 2d * af[i]);
}
if (double.IsNaN(pos.Z))
{
probing.Grbl.IsJobRunning = false;
probing.Program.End((string)FindResource("PositionUnknown"));
return;
}
if (probing.ProbeZ && axisflags != AxisFlags.Z)
{
Position pz = new Position(pos);
pz.X += probing.ProbeDiameter / 2d * af[GrblConstants.X_AXIS];
pz.Y += probing.ProbeDiameter / 2d * af[GrblConstants.Y_AXIS];
if ((ok = !isCancelled && probing.GotoMachinePosition(pz, axisflags)))
{
ok = !isCancelled && probing.WaitForResponse(probing.FastProbe + "Z-" + probing.Depth.ToInvariantString());
ok = ok && !isCancelled && probing.WaitForResponse(probing.RapidCommand + "Z" + probing.LatchDistance.ToInvariantString());
ok = ok && !isCancelled && probing.RemoveLastPosition();
if ((ok = ok && !isCancelled && probing.WaitForResponse(probing.SlowProbe + "Z-" + probing.Depth.ToInvariantString())))
{
pos.Z = probing.Grbl.ProbePosition.Z;
ok = !isCancelled && probing.GotoMachinePosition(probing.StartPosition, AxisFlags.Z);
}
}
}
ok = ok && !isCancelled && probing.GotoMachinePosition(pos, AxisFlags.Y);
ok = ok && !isCancelled && probing.GotoMachinePosition(pos, AxisFlags.X);
if (probing.ProbeZ)
{
axisflags |= AxisFlags.Z;
}
if (ok)
{
if (probing.CoordinateMode == ProbingViewModel.CoordMode.G92)
{
if ((ok = !isCancelled && probing.GotoMachinePosition(pos, AxisFlags.Z)))
{
pos.X = -probing.ProbeOffsetX;
pos.Y = -probing.ProbeOffsetY;
pos.Z = probing.WorkpieceHeight + probing.TouchPlateHeight;
probing.WaitForResponse("G92" + pos.ToString(axisflags));
if (!isCancelled && axisflags.HasFlag(AxisFlags.Z))
{
probing.GotoMachinePosition(probing.StartPosition, AxisFlags.Z);
}
}
}
else
{
pos.X += probing.ProbeOffsetX;
pos.Y += probing.ProbeOffsetY;
pos.Z -= probing.WorkpieceHeight + probing.TouchPlateHeight + probing.Grbl.ToolOffset.Z;
probing.WaitForResponse(string.Format("G10L2P{0}{1}", probing.CoordinateSystem, pos.ToString(axisflags)));
}
}
probing.Program.End((string)FindResource(ok ? "ProbingCompleted" : "ProbingFailed"));
}
if (!probing.Grbl.IsParserStateLive && probing.CoordinateMode == ProbingViewModel.CoordMode.G92)
{
probing.Grbl.ExecuteCommand(GrblConstants.CMD_GETPARSERSTATE);
}
probing.Grbl.IsJobRunning = false;
}