private void DrawArc(GCArc arc, double[] start, GCPlane plane, bool isRelative = false)
{
List <Point3D> points = arc.GeneratePoints(plane, start, ArcResolution, isRelative); // Dynamic resolution
foreach (Point3D point in points)
{
AddCutMove(point);
}
}
private void DrawArc(GCPlane plane, double[] start, double[] stop, double[] ijkValues, bool absoluteIJKMode, bool clockwise)
{
double[] center = updateCenterWithCommand(plane, start, ijkValues, absoluteIJKMode);
List <Point3D> arcpoints = generatePointsAlongArcBDring(plane, start, stop, center, clockwise, 0d, ArcResolution); // Dynamic resolution
foreach (Point3D point in arcpoints)
{
AddCutMove(point);
}
}
public void AddPoint(GCPlane plane, Point3D point)
{
Min[plane.Axis0] = Math.Min(Min[plane.Axis0], point.X);
Max[plane.Axis0] = Math.Max(Max[plane.Axis0], point.X);
Min[plane.Axis1] = Math.Min(Min[plane.Axis1], point.Y);
Max[plane.Axis1] = Math.Max(Max[plane.Axis1], point.Y);
Min[plane.AxisLinear] = Math.Min(Min[plane.AxisLinear], point.Z);
Max[plane.AxisLinear] = Math.Max(Max[plane.AxisLinear], point.Z);
}
public void AddPoint(GCPlane plane, double x, double y, double z)
{
Min[plane.Axis0] = Math.Min(Min[plane.Axis0], x);
Max[plane.Axis0] = Math.Max(Max[plane.Axis0], x);
Min[plane.Axis1] = Math.Min(Min[plane.Axis1], y);
Max[plane.Axis1] = Math.Max(Max[plane.Axis1], y);
Min[plane.AxisLinear] = Math.Min(Min[plane.AxisLinear], z);
Max[plane.AxisLinear] = Math.Max(Max[plane.AxisLinear], z);
}
//-------------
private void DrawArc(GCPlane plane, double[] start, double[] stop, double radius, bool clockwise)
{
double[] center = convertRToCenter(plane, start, stop, radius, false, clockwise);
List <Point3D> arcpoints = generatePointsAlongArcBDring(plane, start, stop, center, clockwise, 0, ArcResolution); // Dynamic resolution
Point3D old_point = arcpoints[0];
arcpoints.RemoveAt(0);
AddCutMove(old_point);
foreach (Point3D point in arcpoints)
{
AddCutMove(point);
}
}
// Try to create an arc :)
public static double[] convertRToCenter(GCPlane plane, double[] start, double[] end, double radius, bool absoluteIJK, bool clockwise)
{
double[] center = new double[2];
// This math is copied from GRBL in gcode.c
double x = end[plane.Axis0] - start[plane.Axis0];
double y = end[plane.Axis1] - start[plane.Axis1];
double h_x2_div_d = 4d * radius * radius - x * x - y * y;
if (h_x2_div_d < 0d)
{
Console.Write("Error computing arc radius.");
}
h_x2_div_d = (-Math.Sqrt(h_x2_div_d)) / Hypotenuse(x, y);
if (!clockwise)
{
h_x2_div_d = -h_x2_div_d;
}
// Special message from gcoder to software for which radius
// should be used.
if (radius < 0d)
{
h_x2_div_d = -h_x2_div_d;
// TODO: Places that use this need to run ABS on radius.
radius = -radius;
}
double offsetX = 0.5d * (x - (y * h_x2_div_d));
double offsetY = 0.5d * (y + (x * h_x2_div_d));
if (!absoluteIJK)
{
center[0] = start[plane.Axis0] + offsetX;
center[1] = start[plane.Axis1] + offsetY;
}
else
{
center[0] = offsetX;
center[1] = offsetY;
}
return(center);
}
static public double[] updateCenterWithCommand(GCPlane plane, double[] initial, double[] ijkValues, bool absoluteIJKMode)
{
double[] newPoint = new double[2];
if (absoluteIJKMode)
{
newPoint[0] = ijkValues[plane.Axis0];
newPoint[1] = ijkValues[plane.Axis1];
}
else
{
newPoint[0] = initial[plane.Axis0] + ijkValues[plane.Axis0];
newPoint[1] = initial[plane.Axis1] + ijkValues[plane.Axis1];
}
return(newPoint);
}
/*
* Generates the points along an arc including the start and end points.
*/
public static List <Point3D> generatePointsAlongArcBDring(GCPlane plane, double[] p1,
double[] p2, double[] center, bool isCw, double radius,
double startAngle, double sweep, int numPoints)
{
Point3D lineEnd = new Point3D();
List <Point3D> segments = new List <Point3D>();
double angle;
double zIncrement = (p2[plane.AxisLinear] - p1[plane.AxisLinear]) / numPoints;
for (int i = 0; i < numPoints; i++)
{
if (isCw)
{
angle = (startAngle - i * sweep / numPoints);
}
else
{
angle = (startAngle + i * sweep / numPoints);
}
if (angle >= Math.PI * 2d)
{
angle = angle - Math.PI * 2d;
}
p1[plane.Axis0] = Math.Cos(angle) * radius + center[0];
p1[plane.Axis1] = Math.Sin(angle) * radius + center[1];
lineEnd.X = p1[0];
lineEnd.Y = p1[1];
lineEnd.Z = p1[2];
p1[plane.AxisLinear] += zIncrement;
segments.Add(lineEnd);
}
lineEnd.X = p2[0];
lineEnd.Y = p2[1];
lineEnd.Z = p2[2];
segments.Add(lineEnd);
return(segments);
}
public GCodeEmulator()
{
coordinateSystems.Clear();
foreach (CoordinateSystem c in GrblWorkParameters.CoordinateSystems)
{
coordinateSystems.Add(c);
}
coordinateSystem = coordinateSystems.Where(x => x.Code == GrblParserState.WorkOffset).FirstOrDefault();
g28 = coordinateSystems.Where(x => x.Code == "G28").FirstOrDefault();
g30 = coordinateSystems.Where(x => x.Code == "G30").FirstOrDefault();
g92 = coordinateSystems.Where(x => x.Code == "G92").FirstOrDefault();
LatheMode = GrblParserState.LatheMode;
isRelative = GrblParserState.DistanceMode == DistanceMode.Incremental;
foreach (int i in AxisFlags.All.ToIndices())
{
offsets[i] = coordinateSystem.Values[i];
origin[i] = g92.Values[i];
}
switch (GrblParserState.Plane)
{
case GCode.Plane.XY:
Plane = new GCPlane(Commands.G17, 0);
break;
case GCode.Plane.XZ:
Plane = new GCPlane(Commands.G18, 0);
break;
case GCode.Plane.YZ:
Plane = new GCPlane(Commands.G19, 0);
break;
}
}
/**
* Generates the points along an arc including the start and end points.
*/
public static List <Point3D> generatePointsAlongArcBDring(GCPlane plane, double[] p1, double[] p2, double[] center, bool isCw, double radius, int arcResolution)
{
double sweep;
// Calculate radius if necessary.
if (radius == 0d)
{
radius = Hypotenuse(p1[plane.Axis0] - center[0], p1[plane.Axis1] - center[1]);
}
// Calculate angles from center.
double startAngle = getAngle(center, p1[plane.Axis0], p1[plane.Axis1]);
double endAngle = getAngle(center, p2[plane.Axis0], p2[plane.Axis1]);
if (startAngle == endAngle)
{
sweep = Math.PI * 2d;
}
else
{
// Fix semantics, if the angle ends at 0 it really should end at 360.
if (endAngle == 0d)
{
endAngle = Math.PI * 2d;
}
// Calculate distance along arc.
if (!isCw && endAngle < startAngle)
{
sweep = ((Math.PI * 2d - startAngle) + endAngle);
}
else if (isCw && endAngle > startAngle)
{
sweep = ((Math.PI * 2d - endAngle) + startAngle);
}
else
{
sweep = Math.Abs(endAngle - startAngle);
}
}
arcResolution = (int)Math.Max(1d, (sweep / (Math.PI * 18d / 180d)));
// arcResolution = (int)Math.Ceiling((sweep * radius) / .1d);
//if (arcDegreeMode && arcPrecision > 0)
//{
// numPoints = qMax(1.0, sweep / (M_PI * arcPrecision / 180));
//}
//else
//{
// if (arcPrecision <= 0 && minArcLength > 0)
// {
// arcPrecision = minArcLength;
// }
// numPoints = (int)ceil(arcLength / arcPrecision);
//}
return(generatePointsAlongArcBDring(plane, p1, p2, center, isCw, radius, startAngle, sweep, arcResolution));
}
public void Render(List <GCodeToken> tokens)
{
var bbox = ((GrblViewModel)DataContext).ProgramLimits;
double lineThickness = bbox.MaxSize / 1000;
double arrowOffset = lineThickness * 30;
double labelOffset = lineThickness * 50;
bool canned = false;
ClearViewport();
coordinateSystems.Clear();
foreach (CoordinateSystem c in GrblWorkParameters.CoordinateSystems)
{
coordinateSystems.Add(c);
}
coordinateSystem = coordinateSystems.Where(x => x.Code == GrblParserState.WorkOffset).FirstOrDefault();
cutCount = 0;
point0 = Machine.StartPosition;
lastType = MoveType.None;
distanceMode = GrblParserState.DistanceMode;
#region Canvas adorners
if (ShowGrid)
{
double wm = bbox.SizeX % TickSize, w = Math.Ceiling(bbox.SizeX - bbox.SizeX % TickSize + TickSize * 2d);
double wh = bbox.SizeY % TickSize, h = Math.Ceiling(bbox.SizeY - bbox.SizeY % TickSize + TickSize * 2d);
Machine.Grid = new GridLinesVisual3D()
{
Center = new Point3D(boffset(bbox.SizeX, bbox.MinX, w, wm) - TickSize, boffset(bbox.SizeY, bbox.MinY, h, wh) - TickSize, 0d),
MinorDistance = 2.5d,
MajorDistance = TickSize,
Width = h,
Length = w,
Thickness = 0.1d,
Fill = AxisBrush
};
viewport.Children.Add(Machine.Grid);
}
if (ShowAxes)
{
Machine.Axes.Children.Add(new ArrowVisual3D()
{
Point2 = new Point3D(bbox.SizeX + arrowOffset, 0.0, 0.0),
Diameter = lineThickness * 5,
Fill = AxisBrush
});
Machine.Axes.Children.Add(new BillboardTextVisual3D()
{
Text = "X",
FontWeight = FontWeights.Bold,
Foreground = AxisBrush,
Position = new Point3D(bbox.SizeX + labelOffset, 0.0, 0.0)
});
Machine.Axes.Children.Add(new ArrowVisual3D()
{
Point2 = new Point3D(0.0, bbox.SizeY + arrowOffset, 0.0),
Diameter = lineThickness * 5,
Fill = AxisBrush
});
Machine.Axes.Children.Add(new BillboardTextVisual3D()
{
Text = "Y",
FontWeight = FontWeights.Bold,
Foreground = AxisBrush,
Position = new Point3D(0.0, bbox.SizeY + labelOffset, 0.0)
});
if (bbox.SizeZ > 0d)
{
Machine.Axes.Children.Add(new ArrowVisual3D()
{
Point1 = new Point3D(0.0, 0.0, bbox.MinZ),
Point2 = new Point3D(0.0, 0.0, bbox.MaxZ + arrowOffset),
Diameter = lineThickness * 5,
Fill = AxisBrush
});
Machine.Axes.Children.Add(new BillboardTextVisual3D()
{
Text = "Z",
FontWeight = FontWeights.Bold,
Foreground = AxisBrush,
Position = new Point3D(0.0, 0.0, bbox.MaxZ + labelOffset)
});
}
viewport.Children.Add(Machine.Axes);
}
if (ShowBoundingBox && bbox.SizeZ > 0d)
{
Machine.BoundingBox = new BoundingBoxWireFrameVisual3D()
{
BoundingBox = new Rect3D(bbox.MinX, bbox.MinY, bbox.MinZ, bbox.SizeX, bbox.SizeY, bbox.SizeZ),
Thickness = 1d,
Color = Colors.LightGreen
};
viewport.Children.Add(Machine.BoundingBox);
}
#endregion
GCodeToken last = new GCodeToken();
foreach (GCodeToken token in tokens)
{
switch (token.Command)
{
case Commands.G0:
{
GCLinearMotion motion = (GCLinearMotion)token;
var pt = toPoint(motion.Values);
if (distanceMode == DistanceMode.Incremental)
{
pt.Offset(point0.X, point0.Y, point0.Z);
}
//if (last.Command == Commands.G1 && (((GCLinearMotion)last).X != point0.X || ((GCLinearMotion)last).Y != point0.Y))
// path.Points.Add(pt);
AddRapidMove(pt);
}
break;
case Commands.G1:
{
GCLinearMotion motion = (GCLinearMotion)token;
var pt = toPoint(motion.Values);
if (distanceMode == DistanceMode.Incremental)
{
pt.Offset(point0.X, point0.Y, point0.Z);
}
//if (last.Command == Commands.G0 && (((GCLinearMotion)last).X != point0.X || ((GCLinearMotion)last).Y != point0.Y))
// path.Points.Add(pt);
AddCutMove(pt);
}
break;
case Commands.G2:
case Commands.G3:
GCArc arc = (GCArc)token;
if (distanceMode == DistanceMode.Incremental)
{
arc.X += point0.X;
arc.Y += point0.Y;
arc.Z += point0.Z;
}
if (arc.IsRadiusMode)
{
DrawArc(plane, point0.ToArray(), arc.Values, arc.R, arc.IsClocwise);
}
else
{
DrawArc(plane, point0.ToArray(), arc.Values, arc.IJKvalues, arc.IJKMode == IJKMode.Absolute, arc.IsClocwise);
}
break;
case Commands.G10:
case Commands.G92:
{
if (token is GCCoordinateSystem)
{
CoordinateSystem csys;
GCCoordinateSystem gcsys = (GCCoordinateSystem)token;
if (gcsys.P == 0)
{
csys = coordinateSystem;
}
else
{
csys = coordinateSystems.Where(x => x.Code == gcsys.Code).FirstOrDefault();
}
for (int i = 0; i < 3; i++)
{
csys.Values[i] = gcsys.Values[i];
if (gcsys.P == 0)
{
offsets[i] = coordinateSystem.Values[i];
}
}
}
}
break;
case Commands.G17:
case Commands.G18:
case Commands.G19:
plane = (GCPlane)token;
break;
//case Commands.G20:
//case Commands.G21:
//case Commands.G50:
//case Commands.G51:
// !! Scaling is taken care of in the parser
// break;
case Commands.G28_1:
case Commands.G30_1:
case Commands.G54:
case Commands.G55:
case Commands.G56:
case Commands.G57:
case Commands.G58:
case Commands.G59:
case Commands.G59_1:
case Commands.G59_2:
case Commands.G59_3:
case Commands.G92_1:
{
string cs = token.Command.ToString().Replace('_', '.');
coordinateSystem = coordinateSystems.Where(x => x.Code == cs).FirstOrDefault();
for (int i = 0; i < 3; i++)
{
offsets[i] = coordinateSystem.Values[i];
}
// CoordinateSystem = GrblWorkParameters.CoordinateSystems();
//GCCoordinateSystem cs = (GCCoordinateSystem)token;
// TODO: handle offsets... Need to read current from grbl
}
break;
case Commands.G80:
canned = false;
break;
case Commands.G81: // TODO: add plane handling
{
GCCannedDrill drill = (GCCannedDrill)token;
uint repeats = distanceMode == DistanceMode.Incremental ? drill.L : 1; // no need to draw absolute repeats(?)
double[] values = new double[3];
for (var i = 0; i < values.Length; i++)
{
values[i] = distanceMode == DistanceMode.Incremental && i < 2 ? 0d : drill.Values[i];
}
if (!canned)
{
canned = true;
if (point0.Z < drill.R)
{
AddRapidMove(toPoint(point0.X, point0.Y, drill.R));
}
}
AddRapidMove(toPoint(drill.X, drill.Y, Math.Max(drill.Z, drill.R)));
do
{
AddCutMove(toPoint(values));
AddRetractMove(toPoint(values[0], values[1], drill.R));
if (repeats > 1)
{
AddRapidMove(toPoint(values[0], values[1], drill.R));
values[0] += drill.X;
values[1] += drill.Y;
AddRapidMove(toPoint(values[0], values[1], drill.R));
}
} while (--repeats > 0);
}
break;
case Commands.G90:
case Commands.G91:
distanceMode = ((GCDistanceMode)token).DistanceMode;
break;
}
last = token;
}
last = null;
Machine.RapidLines = rapidPoints;
Machine.CutLines = linePoints;
Machine.RetractLines = retractPoints;
refreshCamera(bbox);
}
public void Reset()
{
coordinateSystems.Clear();
foreach (CoordinateSystem c in GrblWorkParameters.CoordinateSystems)
{
coordinateSystems.Add(c);
}
toolTable.Clear();
if (GrblInfo.NumTools > 0)
{
foreach (Tool t in GrblWorkParameters.Tools)
{
toolTable.Add(t);
}
}
coordinateSystem = coordinateSystems.Where(x => x.Code == GrblParserState.WorkOffset).FirstOrDefault();
g28 = coordinateSystems.Where(x => x.Code == "G28").FirstOrDefault();
g30 = coordinateSystems.Where(x => x.Code == "G30").FirstOrDefault();
g92 = coordinateSystems.Where(x => x.Code == "G92").FirstOrDefault();
LatheMode = GrblParserState.LatheMode;
isRelative = GrblParserState.DistanceMode == DistanceMode.Incremental;
IsImperial = !GrblParserState.IsMetric;
// TODO: set from parser state
CoolantState = CoolantState.Off;
SpindleState = SpindleState.Off;
LatheMode = LatheMode.Disabled;
DistanceMode = DistanceMode.Absolute;
ToolLengthOffset = ToolLengthOffset.Cancel;
IJKMode = IJKMode.Incremental;
IsScaled = false;
Tool = 0;
Feedrate = 0d;
SelectedTool = null;
// end TODO
foreach (int i in AxisFlags.All.ToIndices())
{
offsets[i] = coordinateSystem == null ? 0d : coordinateSystem.Values[i];
origin[i] = g92 == null ? 0d : g92.Values[i];
machinePos[i] = 0d;
scaleFactors[i] = 1d;
toolOffsets[i] = SelectedTool == null ? 0d : SelectedTool.Values[i];
}
switch (GrblParserState.Plane)
{
case GCode.Plane.XY:
Plane = new GCPlane(Commands.G17, 0);
break;
case GCode.Plane.XZ:
Plane = new GCPlane(Commands.G18, 0);
break;
case GCode.Plane.YZ:
Plane = new GCPlane(Commands.G19, 0);
break;
}
}
public void ApplyRotation(double angle, Vector3 offset, bool compress = false)
{
uint G53lnr = 0;
int precision = GCode.File.Decimals;
GCPlane plane = new GCPlane(GrblParserState.Plane == Plane.XY ? Commands.G17 : Commands.G18, 0);
DistanceMode distanceMode = GrblParserState.DistanceMode;
Vector3 pos = new Vector3(Grbl.GrblViewModel.Position.X, Grbl.GrblViewModel.Position.Y, Grbl.GrblViewModel.Position.Z);
List <GCodeToken> toolPath = new List <GCodeToken>();
toolPath.Add(new GCComment(Commands.Comment, 0, string.Format("{0} degree rotation applied", Math.Round(angle * 180d / Math.PI, 1).ToInvariantString())));
foreach (var token in GCode.File.Tokens)
{
switch (token.Command)
{
case Commands.G0:
case Commands.G1:
{
var motion = token as GCLinearMotion;
if (motion.AxisFlags != AxisFlags.None && G53lnr != token.LineNumber)
{
var target = ToAbsolute(pos, motion.Values);
if (distanceMode == DistanceMode.Incremental)
{
if (!motion.AxisFlags.HasFlag(AxisFlags.X))
{
target = new Vector3(0d, target.Y, 0d);
}
if (!motion.AxisFlags.HasFlag(AxisFlags.Y))
{
target = new Vector3(target.X, 0d, 0d);
}
target = target.RotateZ(0d, 0d, angle).Round(precision);
}
else
{
target = target.RotateZ(offset.X, offset.Y, angle).Round(precision);
}
if (target.X != pos.X)
{
motion.AxisFlags |= AxisFlags.X;
}
if (target.Y != pos.Y)
{
motion.AxisFlags |= AxisFlags.Y;
}
if (distanceMode == DistanceMode.Incremental)
{
pos += target;
}
else
{
pos = target;
}
toolPath.Add(new GCLinearMotion(motion.Command, motion.LineNumber, target.Array, motion.AxisFlags));
}
else
{
G53lnr = 0;
toolPath.Add(new GCLinearMotion(motion.Command, motion.LineNumber, motion.Values, motion.AxisFlags));
}
}
break;
case Commands.G2:
case Commands.G3:
{
if (plane.Plane != Plane.XY)
{
throw new Exception(LibStrings.FindResource("HasG17G18Arcs"));
}
if ((token as GCArc).IsRadiusMode) // for now...
{
throw new Exception(LibStrings.FindResource("HasRadiusArcs"));
}
var arc = token as GCArc;
Vector3 target = ToAbsolute(pos, arc.Values).RotateZ(offset.X, offset.Y, angle).Round(precision);
Vector3 targetijk = arc.IsRadiusMode ? new Vector3(double.NaN, double.NaN, double.NaN) : new Vector3(arc.IJKvalues).RotateZ(0d, 0d, angle).Round(precision);
if (pos.X != target.X)
{
arc.AxisFlags |= AxisFlags.X;
}
if (pos.Y != target.Y)
{
arc.AxisFlags |= AxisFlags.Y;
}
pos = target;
toolPath.Add(new GCArc(arc.Command, arc.LineNumber, pos.Array, arc.AxisFlags, targetijk.Array, arc.IjkFlags, arc.R, arc.IJKMode));
}
break;
case Commands.G5:
{
var spline = token as GCSpline;
pos = new Vector3(spline.X, spline.Y, 0d).RotateZ(offset.X, offset.Y, angle).Round(precision);
var ij = new Vector3(spline.I, spline.J, 0d).RotateZ(offset.X, offset.Y, angle).Round(precision);
var pq = new Vector3(spline.P, spline.Q, 0d).RotateZ(offset.X, offset.Y, angle).Round(precision);
toolPath.Add(new GCSpline(spline.Command, spline.LineNumber, pos.Array, spline.AxisFlags, new double[] { ij.X, ij.Y, pq.X, pq.Y }));
}
break;
case Commands.G17:
case Commands.G18:
case Commands.G19:
plane = token as GCPlane;
toolPath.Add(token);
break;
case Commands.G53:
G53lnr = token.LineNumber; // No rotation for next linear move
toolPath.Add(token);
break;
case Commands.G73:
case Commands.G81:
case Commands.G82:
case Commands.G83:
case Commands.G85:
case Commands.G86:
case Commands.G89:
{
var drill = token as GCCannedDrill;
var target = ToAbsolute(pos, drill.Values).RotateZ(offset.X, offset.Y, angle);
if (pos.X != target.X)
{
drill.AxisFlags |= AxisFlags.X;
}
if (pos.Y != target.Y)
{
drill.AxisFlags |= AxisFlags.Y;
}
if (distanceMode == DistanceMode.Incremental)
{
pos = new Vector3(pos.X + target.X * drill.L, pos.Y + target.Y * drill.L, pos.Z + target.Z);
}
else
{
pos = target;
}
toolPath.Add(new GCCannedDrill(drill.Command, drill.LineNumber, target.Round(precision).Array, drill.AxisFlags, drill.R, drill.L, drill.P, drill.Q));
}
break;
case Commands.G90:
case Commands.G91:
distanceMode = (token as GCDistanceMode).DistanceMode;
toolPath.Add(token);
break;
default:
toolPath.Add(token);
break;
}
}
List <string> gc = GCodeParser.TokensToGCode(toolPath, compress);
// GCodeParser.Save(@"C:\Users\terjeio\Desktop\Probing\file.nc", gc);
GCode.File.AddBlock(string.Format("{0} degree rotation applied: {1}", Math.Round(angle * 180d / Math.PI, 1).ToInvariantString(), Grbl.GrblViewModel.FileName), Core.Action.New);
foreach (string block in gc)
{
GCode.File.AddBlock(block, Core.Action.Add);
}
GCode.File.AddBlock("", Core.Action.End);
}
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;
}
public void Reset()
{
// Sync with controller
if (GrblInfo.IsGrblHAL)
{
GrblParserState.Get();
GrblWorkParameters.Get();
}
else
{
GrblParserState.Get(true);
}
coordinateSystems.Clear();
foreach (CoordinateSystem c in GrblWorkParameters.CoordinateSystems)
{
coordinateSystems.Add(c);
}
toolTable.Clear();
if (GrblInfo.NumTools > 0)
{
foreach (Tool t in GrblWorkParameters.Tools)
{
toolTable.Add(t);
}
}
coordinateSystem = coordinateSystems.Where(x => x.Code == GrblParserState.WorkOffset).FirstOrDefault();
g28 = coordinateSystems.Where(x => x.Code == "G28").FirstOrDefault();
g30 = coordinateSystems.Where(x => x.Code == "G30").FirstOrDefault();
g92 = coordinateSystems.Where(x => x.Code == "G92").FirstOrDefault();
isRelative = GrblParserState.DistanceMode == DistanceMode.Incremental;
IsImperial = !GrblParserState.IsMetric;
CoolantState = GrblParserState.CoolantState;
SpindleState = GrblParserState.SpindleState;
LatheMode = GrblParserState.LatheMode;
DistanceMode = GrblParserState.DistanceMode;
ToolLengthOffset = GrblParserState.ToolLengthOffset;
FeedRateMode = GrblParserState.FeedRateMode;
MotionMode = GrblParserState.MotionMode;
IJKMode = GrblParserState.IJKMode;
IsScaled = GrblParserState.IsActive("G51") != null;
string val = GrblParserState.IsActive("F");
Feedrate = val == null ? 0d : double.Parse(val, CultureInfo.InvariantCulture);
val = GrblParserState.IsActive("S");
_rpm = val == null ? 0d : double.Parse(val, CultureInfo.InvariantCulture);
_tool = GrblParserState.Tool == GrblConstants.NO_TOOL ? 0 : int.Parse(GrblParserState.Tool);
SelectedTool = null;
RetractOldZ = GrblParserState.IsActive("G99") == null;
SpindleRpmMode = GrblParserState.IsActive("G96") == null;
G92Active = GrblParserState.IsActive("G92") != null;
Line = 0;
foreach (int i in AxisFlags.All.ToIndices())
{
offsets[i] = coordinateSystem == null ? 0d : coordinateSystem.Values[i];
origin[i] = g92 == null ? 0d : g92.Values[i];
machinePos[i] = 0d;
scaleFactors[i] = 1d;
toolOffsets[i] = SelectedTool == null ? 0d : SelectedTool.Values[i];
}
switch (GrblParserState.Plane)
{
case GCode.Plane.XY:
Plane = new GCPlane(Commands.G17, 0);
break;
case GCode.Plane.XZ:
Plane = new GCPlane(Commands.G18, 0);
break;
case GCode.Plane.YZ:
Plane = new GCPlane(Commands.G19, 0);
break;
}
}
