private void CreateHeightMap(double gridSizeX, double gridSizeY, Vector2 min, Vector2 max)
{
if (min.X == max.X || min.Y == max.Y)
{
throw new Exception(LibStrings.FindResource("HeigthMapNarrow"));
}
int pointsX = (int)Math.Ceiling((max.X - min.X) / gridSizeX) + 1;
int pointsY = (int)Math.Ceiling((max.Y - min.Y) / gridSizeY) + 1;
if (pointsX < 2 || pointsY < 2)
{
throw new Exception(LibStrings.FindResource("HeigthMapMinSize"));
}
Points = new double?[pointsX, pointsY];
if (max.X < min.X)
{
double a = min.X;
min.X = max.X;
max.X = a;
}
if (max.Y < min.Y)
{
double a = min.Y;
min.Y = max.Y;
max.Y = a;
}
Min = min;
Max = max;
SizeX = pointsX;
SizeY = pointsY;
for (int x = 0; x < SizeX; x++)
{
for (int y = 0; y < SizeY; y++)
{
NotProbed.Add(new Tuple <int, int>(x, y));
}
}
}
public bool ValidateInput(bool z_only)
{
ClearErrors();
if (!z_only && XYClearance + ProbeDiameter / 2d > ProbeDistance)
{
SetError(nameof(XYClearance), LibStrings.FindResource("ErrorProbingDistance"));
SetError(nameof(ProbeDistance), LibStrings.FindResource("ErrorProbingDistance"));
}
if (LatchDistance >= ProbeDistance)
{
SetError(nameof(LatchDistance), LibStrings.FindResource("ErrorLatchDistance"));
SetError(nameof(ProbeDistance), LibStrings.FindResource("ErrorLatchDistance"));
}
return(!HasErrors);
}
public bool Execute(bool go)
{
_isComplete = isProbing = false;
probing.ClearExeStatus();
if (_program.Count > 0)
{
string response;
step = 0;
probing.Positions.Clear();
probing.Machine.Clear();
Comms.com.PurgeQueue();
if (!isRunning)
{
isRunning = true;
probeProtect = GrblInfo.HasSimpleProbeProtect;
Grbl.OnCommandResponseReceived += ResponseReceived;
Grbl.PropertyChanged += Grbl_PropertyChanged;
if (hasPause)
{
probing.PropertyChanged += Probing_PropertyChanged;
}
}
Grbl.IsJobRunning = true;
if (probing.Message == string.Empty)
{
probing.Message = LibStrings.FindResource("Probing");
}
cmd_response = string.Empty;
Grbl.ExecuteCommand(_program[step]);
while (!_isComplete)
{
EventUtils.DoEvents();
if (cmd_response != string.Empty)
{
response = cmd_response;
cmd_response = string.Empty;
if (Grbl.ResponseLogVerbose)
{
Grbl.ResponseLog.Add("PM:" + response);
}
if (response == "ok")
{
step++;
if (step < _program.Count)
{
int i;
//if ((i = _program[step].IndexOf('$')) > 0)
//{
// string rp = _program[step].Substring(i, 2);
// i = GrblInfo.AxisLetterToIndex(rp[1]);
// double val = _positions[_positions.Count - 1].Values[i] + dbl.Parse(_program[step].Substring(i, 3));
// _program[step] = _program[step] + val.ToInvariantString();
//}
if (_program[step].StartsWith("!"))
{
isProbing = false;
_program[step] = _program[step].Substring(1);
probing.RemoveLastPosition();
}
if (_program[step] == "pause")
{
probing.IsPaused = true;
probeOnCycleStart = !probing.Grbl.Signals.Value.HasFlag(Signals.CycleStart);
}
else
{
// This fails with a hang if probe is asserted when it should not be...
//if ((isProbing = _program[step].Contains("G38")) && !IsProbeReady())
// response = "probe!";
//else
Grbl.ExecuteCommand(_program[step]);
}
}
}
if (step == _program.Count || response != "ok")
{
probing.IsSuccess = step == _program.Count && response == "ok";
if (!probing.IsSuccess && response != "probe!")
{
End(LibStrings.FindResource(Grbl.GrblState.State == GrblStates.Alarm ? "FailedAlarm" : "FailedCancelled"));
}
_isComplete = probing.IsCompleted = true;
}
}
}
if (probing.Message == LibStrings.FindResource("Probing"))
{
probing.Message = string.Empty;
}
}
return(probing.IsSuccess);
}
public bool Init(bool check_probe = true)
{
bool?res = null;
IsCancelled = false;
probing.Message = string.Empty;
Grbl.Poller.SetState(0); // Disable status polling during initialization
// Clear error status if set
if (Grbl.GrblError != 0)
{
new Thread(() =>
{
res = WaitFor.AckResponse <string>(
cancellationToken,
null,
a => Grbl.OnResponseReceived += a,
a => Grbl.OnResponseReceived -= a,
1000, () => Grbl.ExecuteCommand(""));
}).Start();
while (res == null)
{
EventUtils.DoEvents();
}
res = null;
}
// Get a status report in order to establish current machine position
new Thread(() =>
{
res = WaitFor.SingleEvent <string>(
cancellationToken,
null,
a => Grbl.OnResponseReceived += a,
a => Grbl.OnResponseReceived -= a,
AppConfig.Settings.Base.PollInterval * 5, () => Comms.com.WriteByte(GrblInfo.IsGrblHAL ? GrblConstants.CMD_STATUS_REPORT_ALL : GrblLegacy.ConvertRTCommand(GrblConstants.CMD_STATUS_REPORT)));
}).Start();
while (res == null)
{
EventUtils.DoEvents();
}
Grbl.Poller.SetState(AppConfig.Settings.Base.PollInterval);
if (Grbl.GrblState.State == GrblStates.Alarm)
{
probing.Message = GrblAlarms.GetMessage(Grbl.GrblState.Substate.ToString());
res = false;
}
if (res == true && check_probe)
{
res = IsProbeReady(false);
}
if (res == true && !(Grbl.GrblState.State == GrblStates.Idle || Grbl.GrblState.State == GrblStates.Tool))
{
probing.Message = LibStrings.FindResource("FailedNotIdle");
res = false;
}
if (res == true && !Grbl.IsMachinePositionKnown)
{
probing.Message = LibStrings.FindResource("FailedNoPos");
res = false;
}
probing.StartPosition.Set(probing.Grbl.MachinePosition);
hasPause = probeOnCycleStart = false;
_program.Clear();
return(res == true);
}
public bool IsProbeReady(bool get_status = true)
{
bool ok = true;
if (get_status) // Check for probe connected and not asserted in next real-time report
{
bool?res = null;
CancellationToken cancellationToken = new CancellationToken();
probeAsserted = probeConnected = true;
// Timeout wait for skipping first report as this may have probe asserted true
new Thread(() =>
{
res = WaitFor.SingleEvent <string>(
cancellationToken,
null,
a => Grbl.OnGrblReset += a,
a => Grbl.OnGrblReset -= a,
AppConfig.Settings.Base.PollInterval * 2 + 50);
}).Start();
while (res == null)
{
EventUtils.DoEvents();
}
res = null;
new Thread(() =>
{
res = WaitFor.SingleEvent <string>(
cancellationToken,
probeCheck,
a => Grbl.OnResponseReceived += a,
a => Grbl.OnResponseReceived -= a,
AppConfig.Settings.Base.PollInterval * 5);
}).Start();
while (res == null)
{
EventUtils.DoEvents();
}
}
else
{
probeAsserted = Grbl.Signals.Value.HasFlag(Signals.Probe);
probeConnected = !Grbl.Signals.Value.HasFlag(Signals.ProbeDisconnected);
}
if (ok && !probeConnected)
{
probing.Message = LibStrings.FindResource("NoProbe");
ok = false;
}
if (ok && probeAsserted)
{
probing.Message = LibStrings.FindResource("ProbeAsserted");
ok = false;
}
return(ok);
}
public void ApplyHeightMap(ProbingViewModel model)
{
HeightMap map = model.HeightMap.Map;
double segmentLength = Math.Min(map.GridX, map.GridY);
int precision = model.Grbl.Precision;
GCPlane plane = new GCPlane(GrblParserState.Plane == Plane.XY ? Commands.G17 : Commands.G18, 0);
DistanceMode distanceMode = GrblParserState.DistanceMode;
Vector3 pos = new Vector3(model.Grbl.Position.X, model.Grbl.Position.Y, model.Grbl.Position.Z);
List <GCodeToken> newToolPath = new List <GCodeToken>();
uint lnr = 1;
foreach (var token in GCode.File.Tokens)
{
switch (token.Command)
{
case Commands.G0:
case Commands.G1:
{
var motion = token as GCLinearMotion;
var m = new Line(motion.AxisFlags);
m.Start = pos;
m.End = pos = ToAbsolute(pos, motion.Values, distanceMode == DistanceMode.Incremental);
m.Rapid = token.Command == Commands.G0;
foreach (Motion subMotion in m.Split(segmentLength))
{
Vector3 target = new Vector3(Math.Round(subMotion.End.X, precision), Math.Round(subMotion.End.Y, precision), Math.Round(subMotion.End.Z + map.InterpolateZ(subMotion.End.X, subMotion.End.Y), precision));
newToolPath.Add(new GCLinearMotion(motion.Command, lnr++, target.Array, motion.AxisFlags | AxisFlags.Z));
}
}
break;
case Commands.G2:
case Commands.G3:
{
if (plane.Plane != Plane.XY)
{
throw new Exception(LibStrings.FindResource("HasRadiusArcs"));
}
var arc = token as GCArc;
double[] center = arc.GetCenter(plane, pos.Array);
double[] ijk = new double[3];
Array.Copy(arc.IJKvalues, ijk, 3);
var m = new Arc();
m.Start = pos;
m.End = pos = ToAbsolute(pos, arc.Values, distanceMode == DistanceMode.Incremental);
m.Direction = token.Command == Commands.G2 ? ArcDirection.CW : ArcDirection.CCW;
m.U = center[0];
m.V = center[1];
m.Plane = ArcPlane.XY;
foreach (Motion subMotion in m.Split(segmentLength))
{
if (!arc.IsRadiusMode)
{
ijk[0] = Math.Round(center[0] - subMotion.Start.X, precision);
ijk[1] = Math.Round(center[1] - subMotion.Start.Y, precision);
}
Vector3 target = new Vector3(Math.Round(subMotion.End.X, precision), Math.Round(subMotion.End.Y, precision), Math.Round(subMotion.End.Z + map.InterpolateZ(subMotion.End.X, subMotion.End.Y), precision));
newToolPath.Add(new GCArc(arc.Command, lnr++, target.Array, arc.AxisFlags | AxisFlags.Z, ijk, arc.IjkFlags, arc.R, arc.IJKMode));
}
}
break;
case Commands.G17:
case Commands.G18:
case Commands.G19:
plane = token as GCPlane;
newToolPath.Add(token);
break;
case Commands.G90:
case Commands.G91:
distanceMode = (token as GCDistanceMode).DistanceMode;
newToolPath.Add(token);
break;
default:
newToolPath.Add(token);
break;
}
}
List <string> gc = GCodeParser.TokensToGCode(newToolPath, AppConfig.Settings.Base.AutoCompress);
// GCodeParser.Save(@"C:\Users\terjeio\Desktop\Probing\file.nc", gc);
GCode.File.AddBlock(string.Format("Heightmap applied: {0}", model.Grbl.FileName), Core.Action.New);
foreach (string block in gc)
{
GCode.File.AddBlock(block, Core.Action.Add);
}
GCode.File.AddBlock("", Core.Action.End);
model.HeightMapApplied = true;
}
