/// <summary>
/// Fits to a given camera at a given distance and scale factor.
/// </summary>
/// <param name="camera"></param>
/// <param name="distanceMode"></param>
/// <param name="distance"></param>
/// <param name="scaleFactor"></param>
public void FitToCamera(Camera camera, DistanceMode distanceMode, float distance, float scaleFactor = 1.0f)
{
Vector2 frustrumSize = Vector2.one;
float distanceFromCamera = 1f;
switch (distanceMode)
{
case DistanceMode.FromCameraPosition:
distanceFromCamera = distance;
frustrumSize = CameraFrustrum.GetFrustrumSizeAt(camera, distanceFromCamera);
break;
case DistanceMode.FromNearClipPlane:
distanceFromCamera = _camera.nearClipPlane + distance;
frustrumSize = CameraFrustrum.GetFrustrumSizeAt(camera, distanceFromCamera);
break;
case DistanceMode.FromFarClipPlane:
distanceFromCamera = _camera.farClipPlane + distance;
frustrumSize = CameraFrustrum.GetFrustrumSizeAt(camera, distanceFromCamera);
break;
}
transform.position = camera.transform.position + camera.transform.forward * distanceFromCamera;
transform.rotation = camera.transform.rotation;
transform.localScale = new Vector3(frustrumSize.x * scaleFactor, frustrumSize.y * scaleFactor, 1f);
}
public TargetTestCase(string name, DistanceMode distanceMode, TargetMode targetMode, DisplayMode displayMode)
{
Name = name;
DistanceMode = distanceMode;
TargetMode = targetMode;
DisplayMode = displayMode;
}
public void Activate(bool activate, ViewType chgMode)
{
if (activate)
{
if (model.CoordinateSystems.Count == 0)
{
// model.CoordinateSystems.Add(model.CoordinateSystem = new CoordinateSystem("Active", "0"));
foreach (var cs in GrblWorkParameters.CoordinateSystems)
{
if (cs.Id > 0 && cs.Id < 9)
{
model.CoordinateSystems.Add(new CoordinateSystem(cs.Code, "0"));
}
if (cs.Id == 9)
{
model.HasCoordinateSystem9 = true;
}
}
model.HasToolTable = GrblInfo.NumTools > 0;
}
GrblParserState.Get(!GrblSettings.IsGrblHAL);
mode = GrblParserState.DistanceMode;
model.Tool = model.Grbl.Tool == GrblConstants.NO_TOOL ? "0" : model.Grbl.Tool;
model.CanProbe = !model.Grbl.Signals.Value.HasFlag(Signals.Probe);
model.HeightMapApplied = GCode.File.HeightMapApplied;
int csid = GrblWorkParameters.GetCoordinateSystem(model.Grbl.WorkCoordinateSystem).Id;
model.CoordinateSystem = csid == 0 || csid >= 9 ? 1 : csid;
if (model.Grbl.IsTloReferenceSet && !double.IsNaN(model.Grbl.TloReference))
{
model.TloReference = model.Grbl.TloReference;
model.ReferenceToolOffset = false;
}
model.Grbl.PropertyChanged += Grbl_PropertyChanged;
probeTriggered = model.Grbl.Signals.Value.HasFlag(Signals.Probe);
DisplayPosition(model.Grbl);
}
else
{
model.Grbl.PropertyChanged -= Grbl_PropertyChanged;
// If probing alarm active unlock
if (model.Grbl.GrblState.State == GrblStates.Alarm && (model.Grbl.GrblState.Substate == 4 || model.Grbl.GrblState.Substate == 5))
{
model.Grbl.ExecuteCommand(GrblConstants.CMD_UNLOCK);
}
else if (model.Grbl.GrblError != 0)
{
model.Grbl.ExecuteCommand(""); // Clear error
}
model.Grbl.ExecuteCommand(mode == DistanceMode.Absolute ? "G90" : "G91");
}
model.Grbl.Poller.SetState(activate ? AppConfig.Settings.Base.PollInterval : 0);
}
public static double CalculateDistance(VectorD vectorA, VectorD vectorB, DistanceMode mode)
{
if (vectorA.Count != vectorB.Count)
{
throw new Exception("Both vectors should have the same number of elemetns in order to calculate a distance between them!");
}
switch (mode)
{
case DistanceMode.Euclidean:
var sum = vectorA.Select((t, i) => Math.Pow(t - vectorB[i], 2.0)).Sum();
return(Math.Sqrt(sum));
case DistanceMode.Manhattan:
var sum2 = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Sum();
return(sum2);
case DistanceMode.Max:
var max = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Max();
return(max);
case DistanceMode.Min:
var min = vectorA.Select((t, i) => Math.Abs(t - vectorB[i])).Min();
return(min);
case DistanceMode.Levenshtein:
var l = LevenshteinDistance(vectorA, vectorB);
return(l);
default:
throw new Exception("Unknown distance mode!");
}
}
public async Task <Distance?> GetDistanceAsync(Location from, Location to, DistanceMode mode = DistanceMode.Driving)
{
from = from.ConvertToGcj02();
to = to.ConvertToGcj02();
if (mode == DistanceMode.Straight)
{
return(to.StraightDistanceTo(from));
}
var url = "https://apis.map.qq.com/ws/distance/v1/matrix/" +
$"?key={_options.Key}" +
$"&mode={mode.ToLower()}" +
$"&from={from}" +
$"&to={to}";
var x = await GetApiResultAsync(url);
return(x == null ? null : new Distance {
From = from,
To = to,
Mode = mode,
Meters = (int)x.rows[0].elements[0].distance,
TravelTimeEstimate = TimeSpan.FromSeconds((int?)x.rows[0].elements[0].duration ?? 0),
});
}
public TestCase(int targetsCount, ColorMode color, DisplayMode displayMode, int maxTargetScale,
int minTargetScale, DistanceMode distanceMode, float radius, string name, TargetMode targetMode)
: base(name, distanceMode, targetMode, displayMode)
{
TargetsCount = targetsCount;
Color = color;
MaxTargetScale = maxTargetScale;
MinTargetScale = minTargetScale;
Radius = radius;
}
//--------------------------------------------------------------------------------------------------
#endregion
#region Create
LinearArray()
{
Name = "Linear Array";
_Plane = PlaneType.XY;
_Rotation = 0;
_Quantity1 = 1;
_Distance1 = 1.0;
_DistanceMode1 = DistanceMode.Interval;
_Alignment1 = AlignmentMode.First;
_Quantity2 = 1;
_Distance2 = 1.0;
_DistanceMode2 = DistanceMode.Interval;
_Alignment2 = AlignmentMode.First;
_Border = false;
}
//TODO: Refactor this!!!
public Tuple <Vector2, float> GetRandomPosition(DistanceMode mode, float predefinedRadius, TargetData targetData, int group, Vector2Int scaleRange)
{
switch (mode)
{
case DistanceMode.Random:
{
float scale = GetRandomScale(scaleRange);
InitializeRanges(scale);
int pos = _rng.Next(_maxRange);
int y = pos % _boardHeightPoints - _boardHeightPoints / 2;
int x = pos / _boardHeightPoints - _boardWidthPoints / 2;
return(new Tuple <Vector2, float>(new Vector2(x / _pointsPerUnit, y / _pointsPerUnit), scale));
}
case DistanceMode.EqualDistance:
{
float scale = GetRandomScale(scaleRange);
InitializeRanges(scale);
return(new Tuple <Vector2, float>(GetRandomPositionAtRange(predefinedRadius, targetData.XUnitPosition, targetData.YUnitPosition), scale));
}
case DistanceMode.LinRegOptimised:
{
// Generate desired Difficulty Index:
Vector2 range = GetIDRangeFromGroup(group);
float desiredID = GetRandomFloatFromRange(range);
// Calculate bounds of target width:
int upperBound = MaxTargetScale(scaleRange.y, targetData.XUnitPosition, targetData.YUnitPosition,
desiredID);
Vector2Int adjustedScaleRange =
new Vector2Int(scaleRange.x, (scaleRange.y > upperBound ? upperBound : scaleRange.y));
// Generate scale and target size:
float scale = GetRandomScale(adjustedScaleRange);
float targetSize = InitializeRanges(scale);
// Find all posible target locations and pick one:
float radius = (float)(targetSize * Math.Pow(2, desiredID - 1));
return(new Tuple <Vector2, float>(
GetRandomPositionAtRange(radius, targetData.XUnitPosition, targetData.YUnitPosition), scale));
}
default:
return(new Tuple <Vector2, float>(new Vector2(0, 0), 0.0f));
}
}
public void Activate(bool activate, ViewType chgMode)
{
if (activate)
{
if (model.CoordinateSystems.Count == 0)
{
// model.CoordinateSystems.Add(model.CoordinateSystem = new CoordinateSystem("Active", "0"));
foreach (var cs in GrblWorkParameters.CoordinateSystems)
{
if (cs.Id > 0 && cs.Id < 9)
{
model.CoordinateSystems.Add(new CoordinateSystem(cs.Code, "0"));
}
if (cs.Id == 9)
{
model.HasCoordinateSystem9 = true;
}
}
model.HasToolTable = GrblInfo.NumTools > 0;
}
GrblParserState.Get();
mode = GrblParserState.DistanceMode;
model.Tool = model.Grbl.Tool == GrblConstants.NO_TOOL ? "0" : model.Grbl.Tool;
model.CanProbe = !model.Grbl.Signals.Value.HasFlag(Signals.Probe);
model.HeightMapApplied = GCode.File.HeightMapApplied;
int csid = GrblWorkParameters.GetCoordinateSystem(model.Grbl.WorkCoordinateSystem).Id;
model.CoordinateSystem = csid == 0 || csid >= 9 ? 1 : csid;
model.Grbl.PropertyChanged += Grbl_PropertyChanged;
}
else
{
model.Grbl.PropertyChanged -= Grbl_PropertyChanged;
if (model.Grbl.GrblError != 0)
{
model.Grbl.ExecuteCommand(""); // Clear error
}
model.Grbl.ExecuteCommand(mode == DistanceMode.Absolute ? "G90" : "G91");
}
model.Grbl.Poller.SetState(activate ? AppConfig.Settings.Base.PollInterval : 0);
}
//--------------------------------------------------------------------------------------------------
double _CalculateOffset(DistanceMode distanceMode, uint quantity, double distance, double extents)
{
switch (distanceMode)
{
case DistanceMode.Interval:
return(distance);
case DistanceMode.Spacing:
return(extents + distance);
case DistanceMode.Extent:
return(distance / (quantity - 1));
case DistanceMode.OverallExtent:
return((distance - extents) / (quantity - 1));
default:
throw new ArgumentOutOfRangeException(nameof(distanceMode), distanceMode, null);
}
}
public async Task <Distance[]?> GetDistancesAsync(Location from, IEnumerable <Location> toMany, DistanceMode mode = DistanceMode.Driving)
{
from = from.ConvertToGcj02();
if (mode == DistanceMode.Straight)
{
return(toMany.Select(to => to.StraightDistanceTo(from)).ToArray());
}
var url = "https://apis.map.qq.com/ws/distance/v1/matrix/" +
$"?key={_options.Key}" +
$"&mode={mode.ToLower()}" +
$"&from={from}" +
$"&to={string.Join(';', toMany.Select(x => x.ConvertToGcj02().ToString()))}";
var x = await GetApiResultAsync(url);
if (x == null)
{
return(null);
}
var n = toMany.Count();
var i = 0;
var results = new Distance[n];
foreach (var to in toMany)
{
results[i] = new Distance {
From = from,
To = to,
Mode = mode,
Meters = x.rows[0].elements[i].distance,
TravelTimeEstimate = TimeSpan.FromSeconds((int?)x.rows[0].elements[i].duration ?? 0),
};
i++;
}
return(results);
}
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 SetMode(DistanceMode mode)
{
_distanceMode = mode;
}
public static void SetMode(DistanceMode mode)
{
mDistanceMode = mode;
}
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 MoveArc(float x, float y, float z, float i, float j, float k, float radius, bool clockwise, DistanceMode distanceMode)
{
// Only XY plane supported for now.
Point start = new Point(_currentX * StepsPerMmX, _currentY * StepsPerMmY), end;
ArcInterpolation arc = null;
if (radius == float.MinValue)
{
// Center format arc.
if (i == float.MinValue && j == float.MinValue)
{
throw new Exception("G2/3: I and J are missing");
}
if (i == float.MinValue)
{
i = 0;
}
if (j == float.MinValue)
{
j = 0;
}
if (x == float.MinValue)
{
x = _currentX;
}
if (y == float.MinValue)
{
y = _currentY;
}
if (z == float.MinValue)
{
z = _currentZ;
}
Point center = new Point((_currentX + i) * StepsPerMmX, (_currentY + j) * StepsPerMmY);
end = new Point(x * StepsPerMmX, y * StepsPerMmY);
arc = new ArcInterpolation(start, center, end, clockwise);
}
else
{
// Radius format arc
// XYZ are the endpoint. R is the radius.
if (x == float.MinValue && y == float.MinValue)
{
throw new Exception("G2/3: X and Y are missing");
}
if (x == float.MinValue)
{
x = _currentX;
}
if (y == float.MinValue)
{
y = _currentY;
}
if (distanceMode == DistanceMode.Absolute)
{
end = new Point(x * StepsPerMmX, y * StepsPerMmY);
}
else
{
end = new Point((_currentX + x) * StepsPerMmX, (_currentY + y) * StepsPerMmY);
}
arc = new ArcInterpolation(start, end, radius, clockwise);
}
if (arc == null)
{
throw new Exception("G2/3: could not find an arc solution");
}
const float ArcStep = 0.05f; // Make sure it's a "multiple" of 1.0f.
int currentStepsX = (int)start.X;
int currentStepsY = (int)start.Y;
for (float t = ArcStep; t <= 1.0 + (ArcStep / 2); t += ArcStep)
{
Point target = arc.GetArcPoint(t);
// Only XY supported
int targetX = (int)target.X;
int targetY = (int)target.Y;
int dx = (int)target.X - currentStepsX;
int dy = (int)target.Y - currentStepsY;
MoveAxes(dx, dy);
currentStepsX += dx;
currentStepsY += dy;
}
_currentX = currentStepsX / StepsPerMmX;
_currentY = currentStepsY / StepsPerMmY;
}
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);
}
// set distance mode to Short, Medium, or Long
// based on VL53L1_SetDistanceMode()
private bool SetDistanceMode(DistanceMode mode)
{
switch (mode)
{
case DistanceMode.Short:
// from VL53L1_preset_mode_standard_ranging_short_range()
// timing config
WriteReg(RegAddr.RangeConfigVcselPeriodA, 0x07);
WriteReg(RegAddr.RangeConfigVcselPeriodB, 0x05);
WriteReg(RegAddr.RangeConfigValidPhaseHigh, 0x38);
// dynamic config
WriteReg(RegAddr.SdConfigWoiSd0, 0x07);
WriteReg(RegAddr.SdConfigWoiSd1, 0x05);
WriteReg(RegAddr.SdConfigInitialPhaseSd0, 6); // tuning parm default
WriteReg(RegAddr.SdConfigInitialPhaseSd1, 6); // tuning parm default
break;
case DistanceMode.Medium:
// from VL53L1_preset_mode_standard_ranging()
// timing config
WriteReg(RegAddr.RangeConfigVcselPeriodA, 0x0B);
WriteReg(RegAddr.RangeConfigVcselPeriodB, 0x09);
WriteReg(RegAddr.RangeConfigValidPhaseHigh, 0x78);
// dynamic config
WriteReg(RegAddr.SdConfigWoiSd0, 0x0B);
WriteReg(RegAddr.SdConfigWoiSd1, 0x09);
WriteReg(RegAddr.SdConfigInitialPhaseSd0, 10); // tuning parm default
WriteReg(RegAddr.SdConfigInitialPhaseSd1, 10); // tuning parm default
break;
case DistanceMode.Long: // long
// from VL53L1_preset_mode_standard_ranging_long_range()
// timing config
WriteReg(RegAddr.RangeConfigVcselPeriodA, 0x0F);
WriteReg(RegAddr.RangeConfigVcselPeriodB, 0x0D);
WriteReg(RegAddr.RangeConfigValidPhaseHigh, 0xB8);
// dynamic config
WriteReg(RegAddr.SdConfigWoiSd0, 0x0F);
WriteReg(RegAddr.SdConfigWoiSd1, 0x0D);
WriteReg(RegAddr.SdConfigInitialPhaseSd0, 14); // tuning parm default
WriteReg(RegAddr.SdConfigInitialPhaseSd1, 14); // tuning parm default
break;
default:
// unrecognized mode - do nothing
return(false);
}
// save mode so it can be returned by getDistanceMode()
this.distanceMode = mode;
return(true);
}
internal void MachineSetMode(DistanceMode mode)
{
machine.SetMode(mode);
}