public IEnumerable <RunAction> Execute(List <GCodeToken> Tokens) { foreach (GCodeToken token in Tokens) { if (action.LineNumber != token.LineNumber) { action.IsInMachineCoord = false; } action.IsInMachineCoord = false; action.Token = token; action.IsRetract = action.IsSpindleSynced = false; switch (token.Command) { case Commands.G0: case Commands.G1: { var motion = token as GCLinearMotion; setEndP(motion.Values, motion.AxisFlags); } break; case Commands.G2: case Commands.G3: { var arc = token as GCArc; setEndP(arc.Values, arc.AxisFlags); } break; case Commands.G5: { var spline = token as GCSpline; setEndP(spline.Values, spline.AxisFlags); } break; case Commands.G7: LatheMode = LatheMode.Diameter; break; case Commands.G8: LatheMode = LatheMode.Radius; break; case Commands.G10: { if (token is GCCoordinateSystem) { CoordinateSystem csys; GCCoordinateSystem gcsys = token as GCCoordinateSystem; if (gcsys.P == 0) { csys = coordinateSystem; } else { csys = coordinateSystems.Where(x => x.Code == gcsys.Code).FirstOrDefault(); } foreach (int i in gcsys.AxisFlags.ToIndices()) { csys.Values[i] = gcsys.Values[i]; if (gcsys.P == 0) { offsets[i] = csys.Values[i]; } } } } break; case Commands.G17: case Commands.G18: case Commands.G19: Plane = (GCPlane)token; break; case Commands.G20: // Strip G20 for now - Tokens are metric and needs to be transformed back... //IsMetric = false; action.Token.Command = Commands.Undefined; break; case Commands.G21: IsMetric = true; break; case Commands.G28: { var motion = token as GCLinearMotion; AxisFlags axisFlags; if (motion.AxisFlags != AxisFlags.None) { axisFlags = motion.AxisFlags; setEndP(motion.Values, motion.AxisFlags); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } else { axisFlags = AxisFlags.All; } foreach (int i in axisFlags.ToIndices()) { machinePos[i] = g28.Values[i] - offsets[i] - origin[i]; } action.End = machinePos.Point3D; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.All); yield return(action); action.Start = action.End; } break; case Commands.G28_1: { for (int i = 0; i < g28.Values.Length; i++) { g28.Values[i] = offsets[i]; } } break; case Commands.G30: { var motion = token as GCLinearMotion; AxisFlags axisFlags; if (motion.AxisFlags != AxisFlags.None) { axisFlags = motion.AxisFlags; setEndP(motion.Values, motion.AxisFlags); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } else { axisFlags = AxisFlags.All; } foreach (int i in axisFlags.ToIndices()) { machinePos[i] = g30.Values[i] - offsets[i] - origin[i]; } action.End = machinePos.Point3D; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.All); yield return(action); action.Start = action.End; } break; case Commands.G30_1: { for (int i = 0; i < g30.Values.Length; i++) { g30.Values[i] = offsets[i]; } } break; case Commands.G33: { var motion = token as GCSyncMotion; setEndP(motion.Values, motion.AxisFlags); action.IsSpindleSynced = true; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.IsSpindleSynced = false; action.Start = action.End; } break; case Commands.G53: { var motion = token as GCAbsLinearMotion; action.IsInMachineCoord = true; if (action.IsInMachineCoord) { foreach (int i in motion.AxisFlags.ToIndices()) { machinePos[i] = motion.Values[i] - offsets[i] - origin[i]; } action.End = machinePos.Point3D; } action.Token = new GCLinearMotion(motion.Motion, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } break; 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: { string cs = token.Command.ToString().Replace('_', '.'); coordinateSystem = coordinateSystems.Where(x => x.Code == cs).FirstOrDefault(); foreach (int i in AxisFlags.All.ToIndices()) // GrblInfo.AxisFlags? { offsets[i] = coordinateSystem.Values[i]; } // CoordinateSystem = GrblWorkParameters.CoordinateSystems(); //GCCoordinateSystem cs = (GCCoordinateSystem)token; // TODO: handle offsets... Need to read current from grbl } break; case Commands.G76: { var thread = token as GCThreadingMotion; uint pass = 1, passes = 0; double doc = thread.InitialDepth, thread_length, main_taper_height = 0d; double t_end_taper_length = thread.TaperLength; double end_tapers = thread.ThreadTaper == ThreadTaper.None ? 0d : (thread.ThreadTaper == ThreadTaper.Both ? 2d : 1d); double infeed_factor = Math.Tan(thread.InfeedAngle * Math.PI / 180d), infeed_offset = 0d; double target_z = thread.Z + thread.Depth * infeed_factor, start_z = action.Start.Z; if (thread.Z > action.Start.Z) { infeed_factor = -infeed_factor; } var origin = action.End; // Calculate number of passes while (thread.CalculateDOC(++passes) < thread.Depth) { ; } passes += thread.SpringPasses + 1; // TODO: skip rendering of spring passes? if ((thread_length = thread.Z - action.Start.Z) > 0.0f) { t_end_taper_length = -t_end_taper_length; } thread_length += thread.TaperLength * end_tapers; //if (thread->main_taper_height != 0.0f) // thread->main_taper_height = thread->main_taper_height * thread_length / (thread_length - thread->end_taper_length * end_taper_factor); // Initial Z-move for compound slide angle offset. if (infeed_factor != 0d) { infeed_offset = doc * infeed_factor; action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; action.End.Z -= infeed_offset; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } while (--passes > 0) { //action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; //action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); //yield return action; //action.Start = action.End; // Cut thread pass // 1. Entry taper if (thread.ThreadTaper.HasFlag(ThreadTaper.Entry)) { action.IsSpindleSynced = true; // TODO: move this segment outside of synced motion? action.End.X = origin.X + (thread.Peak + doc - thread.Depth) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; action.End.X = origin.X + (thread.Peak + doc) * thread.CutDirection; action.End.Z -= t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } else { // 2. Rapid to DOC action.End.X = origin.X + (thread.Peak + doc) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; action.IsSpindleSynced = true; } // 3. Exit taper if (thread.ThreadTaper.HasFlag(ThreadTaper.Exit)) { action.End.Z = target_z - infeed_offset + t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; action.End.X = origin.X + (thread.Peak + doc - thread.Depth) * thread.CutDirection; action.End.Z -= t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } else { // 2. Main part action.End.Z = target_z - infeed_offset; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; } action.IsSpindleSynced = false; if (passes > 1) { // Get DOC of next pass. doc = Math.Min(thread.CalculateDOC(++pass), thread.Depth); // 4. Retract action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; // 5. Back to start, add compound slide angle offset when commanded. infeed_offset = infeed_factor != 0d ? doc * infeed_factor : 0d; action.End.Z = origin.Z - infeed_offset; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; } else { // 6. Retract to target position doc = thread.Depth; action.End.X = origin.X; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; } } } break; case Commands.G80: canned = false; break; case Commands.G73: case Commands.G81: case Commands.G82: case Commands.G83: case Commands.G85: case Commands.G86: case Commands.G89: // TODO: add plane handling { bool wasRelative = isRelative; GCCannedDrill drill = (token as GCCannedDrill); double r = isRelative ? action.End.Z + drill.R : drill.R; double z = isRelative ? action.End.Z + drill.Z : drill.Z; uint repeats = DistanceMode == DistanceMode.Incremental ? drill.L : 1; // no need to draw absolute repeats(?) setEndP(drill.Values, AxisFlags.XY); isRelative = false; if (!canned) { // canned = true; if (action.End.Z < r) { double[] start = new double[] { action.Start.X, action.Start.Y, r }; setEndP(start, AxisFlags.Z); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, start, AxisFlags.Z); yield return(action); action.Start = action.End; } } double[] values = new double[] { action.End.X, action.End.Y, action.End.Z }; setEndP(values, AxisFlags.X | AxisFlags.Y); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.XY); yield return(action); action.Start = action.End; do { values[2] = z; setEndP(values, AxisFlags.Z); action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, values, AxisFlags.Z); yield return(action); action.Start = action.End; values[2] = r; setEndP(values, AxisFlags.Z); action.IsRetract = true; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.Z); yield return(action); action.Start = action.End; action.IsRetract = false; if (repeats > 1) { values[0] += drill.X; values[1] += drill.Y; setEndP(values, AxisFlags.X | AxisFlags.Y); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.XY); yield return(action); action.Start = action.End; } } while (--repeats > 0); isRelative = wasRelative; action.Token = token; } break; case Commands.G90: case Commands.G91: isRelative = (token as GCDistanceMode).DistanceMode == DistanceMode.Incremental; break; case Commands.G92: { var cs = token as GCCoordinateSystem; foreach (int i in cs.AxisFlags.ToIndices()) { origin[i] = g92.Values[i] = cs.Values[i]; } } break; case Commands.G92_1: { for (int i = 0; i < origin.Length; i++) { origin[i] = g92.Values[i] = 0d; } } break; case Commands.G92_2: { for (int i = 0; i < origin.Length; i++) { origin[i] = 0d; } } break; case Commands.M3: case Commands.M4: case Commands.M5: SpindleState = (token as GCSpindleState).SpindleState; break; case Commands.M7: case Commands.M8: case Commands.M9: CoolantState = (token as GCCoolantState).CoolantState; break; case Commands.Feedrate: Feedrate = (token as GCFeedrate).Feedrate; break; case Commands.SpindleRPM: _rpm = (token as GCSpindleRPM).SpindleRPM; break; case Commands.M61: case Commands.ToolSelect: Tool = (token as GCToolSelect).Tool; break; } if (action.Token.Command != Commands.Undefined) { yield return(action); } action.Start = action.End; } }
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 IEnumerable <RunAction> Execute(List <GCodeToken> Tokens) { Reset(); foreach (GCodeToken token in Tokens) { if (action.LineNumber != token.LineNumber) { action.IsInMachineCoord = false; } action.IsInMachineCoord = false; action.Token = token; action.IsRetract = action.IsSpindleSynced = false; switch (token.Command) { // G0, G1: Linear Move case Commands.G0: case Commands.G1: { var motion = token as GCLinearMotion; setEndP(motion.Values, motion.AxisFlags); } break; // G2, G3: Arc Move case Commands.G2: case Commands.G3: { var arc = token as GCArc; setEndP(arc.Values, arc.AxisFlags); } break; // G5: Cubic Spline case Commands.G5: { var spline = token as GCSpline; setEndP(spline.Values, spline.AxisFlags); } break; // G7: Lathe Diameter Mode case Commands.G7: LatheMode = LatheMode.Diameter; break; // G8: Lathe Radius Mode case Commands.G8: LatheMode = LatheMode.Radius; break; // G10: Set Coordinate System case Commands.G10: if (token is GCCoordinateSystem) { CoordinateSystem csys; GCCoordinateSystem gcsys = token as GCCoordinateSystem; if (gcsys.P == 0) { csys = coordinateSystem; } else { csys = coordinateSystems.Where(x => x.Code == gcsys.Code).FirstOrDefault(); } foreach (int i in gcsys.AxisFlags.ToIndices()) { csys.Values[i] = gcsys.Values[i]; if (gcsys.P == 0) { offsets[i] = csys.Values[i]; } } } break; // G17: XY Plane Select case Commands.G17: // G18: ZX Plane Select case Commands.G18: // G19: YZ Plane Select case Commands.G19: Plane = (GCPlane)token; break; // G21: Metric Units case Commands.G20: // Strip G20 for now - Tokens are metric and needs to be transformed back... IsImperial = true; //for (int i = 0; i < scaleFactors.Length; i++) // scaleFactors[i] = 25.4d; action.Token.Command = Commands.Undefined; // Strip G20 - need to implement unscale... break; // G21 Imperial (inches) Units case Commands.G21: IsImperial = false; //for (int i = 0; i < scaleFactors.Length; i++) // scaleFactors[i] = 1d; break; // G28: Goto Predefined Position case Commands.G28: { var motion = token as GCLinearMotion; AxisFlags axisFlags; if (motion.AxisFlags != AxisFlags.None) { axisFlags = motion.AxisFlags; setEndP(motion.Values, motion.AxisFlags); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } else { axisFlags = AxisFlags.All; } foreach (int i in axisFlags.ToIndices()) { machinePos[i] = g28.Values[i]; // - offsets[i] - origin[i]; } action.End = machinePos.Point3D; // action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.All); action.Token = new GCAbsLinearMotion(token.Command, Commands.G0, token.LineNumber, machinePos.Array, axisFlags); yield return(action); action.Start = action.End; } break; // G28.1: Set Predefined Position case Commands.G28_1: { for (int i = 0; i < g28.Values.Length; i++) { g28.Values[i] = offsets[i]; } } break; // G30: Goto Predefined Position case Commands.G30: { var motion = token as GCLinearMotion; AxisFlags axisFlags; if (motion.AxisFlags != AxisFlags.None) { axisFlags = motion.AxisFlags; setEndP(motion.Values, motion.AxisFlags); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } else { axisFlags = AxisFlags.All; } foreach (int i in axisFlags.ToIndices()) { machinePos[i] = g30.Values[i]; // - offsets[i] - origin[i]; } action.End = machinePos.Point3D; // action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.All); action.Token = new GCAbsLinearMotion(token.Command, Commands.G0, token.LineNumber, machinePos.Array, axisFlags); yield return(action); action.Start = action.End; } break; // G30.1: Set Predefined Position case Commands.G30_1: { for (int i = 0; i < g30.Values.Length; i++) { g30.Values[i] = offsets[i]; } } break; // G33: Spindle Synchronized Motion case Commands.G33: { var motion = token as GCSyncMotion; setEndP(motion.Values, motion.AxisFlags); action.IsSpindleSynced = true; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.IsSpindleSynced = false; action.Start = action.End; } break; // G38.x: Probing motion case Commands.G38_2: case Commands.G38_3: case Commands.G38_4: case Commands.G38_5: if (translate) { var motion = token as GCLinearMotion; setEndP(motion.Values, motion.AxisFlags); action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, motion.AxisFlags); } break; // G43: Tool Length Offset case Commands.G43: SetToolOffset(token as GCToolOffset); break; // G43.1: Dynamic Tool Length Offset case Commands.G43_1: DynamicToolOffset(token as GCToolOffsets); break; // G43.2: Apply additional Tool Length Offset case Commands.G43_2: AddToolOffset(token as GCToolOffset); break; // G49: Cancel Tool Length Compensation case Commands.G49: CancelToolCompensation(); break; // G50: Cancel Scaling case Commands.G50: { IsScaled = false; //for (int i = 0; i < scaleFactors.Length; i++) // scaleFactors[i] = IsImperial ? 25.4d : 1d; for (int i = 0; i < scaleFactors.Length; i++) { scaleFactors[i] = 1d; } } break; case Commands.G51: { IsScaled = false; //var scale = token as GCScaling; ////foreach (int i in scale.AxisFlags.ToIndices()) //// scaleFactors[i] = scale.Values[i] * (IsImperial ? 25.4d : 1d); //foreach (int i in scale.AxisFlags.ToIndices()) // scaleFactors[i] = scale.Values[i]; //for (int i = 0; i < scaleFactors.Length; i++) // IsScaled |= scaleFactors[i] != 1d; // Strip G20 for now - Tokens are already scaled and needs to be transformed back... action.Token.Command = Commands.Undefined; // Strip G51 - need to implement unscale... } break; // G53: Move in Machine Coordinates case Commands.G53: { var motion = token as GCAbsLinearMotion; action.IsInMachineCoord = true; if (action.IsInMachineCoord) { foreach (int i in motion.AxisFlags.ToIndices()) { machinePos[i] = motion.Values[i] - offsets[i] - origin[i]; } action.End = machinePos.Point3D; } action.Token = new GCLinearMotion(motion.Motion, token.LineNumber, machinePos.Array, motion.AxisFlags); yield return(action); action.Start = action.End; } break; // G54-G59.3: Select Coordinate System 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: { string cs = token.Command.ToString().Replace('_', '.'); coordinateSystem = coordinateSystems.Where(x => x.Code == cs).FirstOrDefault(); foreach (int i in AxisFlags.All.ToIndices()) // GrblInfo.AxisFlags? { offsets[i] = coordinateSystem.Values[i]; } // CoordinateSystem = GrblWorkParameters.CoordinateSystems(); //GCCoordinateSystem cs = (GCCoordinateSystem)token; // TODO: handle offsets... Need to read current from grbl } break; // G76: Threading Cycle case Commands.G76: if (translate) { var thread = token as GCThreadingMotion; uint pass = 1, passes = 0; double doc = thread.InitialDepth, thread_length, main_taper_height = 0d; double t_end_taper_length = thread.TaperLength; double end_tapers = thread.ThreadTaper == ThreadTaper.None ? 0d : (thread.ThreadTaper == ThreadTaper.Both ? 2d : 1d); double infeed_factor = Math.Tan(thread.InfeedAngle * Math.PI / 180d), infeed_offset = 0d; double target_z = thread.Z + thread.Depth * infeed_factor, start_z = action.Start.Z + thread.Depth * infeed_factor; if (thread.Z > action.Start.Z) { infeed_factor = -infeed_factor; } var origin = action.End; // Calculate number of passes while (thread.CalculateDOC(++passes) < thread.Depth) { ; } passes += thread.SpringPasses + 1; // TODO: skip rendering of spring passes? if ((thread_length = thread.Z - action.Start.Z) > 0.0f) { t_end_taper_length = -t_end_taper_length; } thread_length += thread.TaperLength * end_tapers; //if (thread->main_taper_height != 0.0f) // thread->main_taper_height = thread->main_taper_height * thread_length / (thread_length - thread->end_taper_length * end_taper_factor); // Initial Z-move for compound slide angle offset. if (infeed_factor != 0d) { infeed_offset = doc * infeed_factor; action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; action.End.Z = start_z - infeed_offset; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } while (--passes > 0) { //action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; //action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); //yield return action; //action.Start = action.End; // Cut thread pass // 1. Entry taper if (thread.ThreadTaper.HasFlag(ThreadTaper.Entry)) { action.IsSpindleSynced = true; // TODO: move this segment outside of synced motion? action.End.X = origin.X + (thread.Peak + doc - thread.Depth) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; action.End.X = origin.X + (thread.Peak + doc) * thread.CutDirection; action.End.Z -= t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } else { // 2. Rapid to DOC action.End.X = origin.X + (thread.Peak + doc) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; action.IsSpindleSynced = true; } // 3. Exit taper if (thread.ThreadTaper.HasFlag(ThreadTaper.Exit)) { action.End.Z = target_z - infeed_offset + t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; action.End.X = origin.X + (thread.Peak + doc - thread.Depth) * thread.CutDirection; action.End.Z -= t_end_taper_length; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.XZ); yield return(action); action.Start = action.End; } else { // 2. Main part action.End.Z = target_z - infeed_offset; action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; } action.IsSpindleSynced = false; if (passes > 1) { // Get DOC of next pass. doc = Math.Min(thread.CalculateDOC(++pass), thread.Depth); // 4. Retract action.End.X = origin.X + (doc - thread.Depth) * thread.CutDirection; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; // 5. Back to start, add compound slide angle offset when commanded. infeed_offset = infeed_factor != 0d ? doc * infeed_factor : 0d; action.End.Z = start_z - infeed_offset; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.Z); yield return(action); action.Start = action.End; } else { // 6. Retract to target position doc = thread.Depth; action.End.X = origin.X; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, machinePos.Array, AxisFlags.X); yield return(action); action.Start = action.End; } } } break; // G80: Cancel Canned Cycle case Commands.G80: canned = false; break; // G73: Drilling Cycle with Chip Breaking case Commands.G73: // G81: Drilling Cycle case Commands.G81: // G82: Drilling Cycle, Dwell case Commands.G82: // G83: Peck Drilling Cycle case Commands.G83: // G85: Boring Cycle, Feed Out case Commands.G85: // G86: Boring Cycle, Spindle Stop, Rapid Move Out case Commands.G86: // G89: Boring Cycle, Dwell, Feed Out case Commands.G89: // TODO: add plane handling if (translate) { bool wasRelative = isRelative; GCCannedDrill drill = (token as GCCannedDrill); double r = isRelative ? action.End.Z + drill.R : drill.R; double z = isRelative ? action.End.Z + drill.Z : drill.Z; uint repeats = DistanceMode == DistanceMode.Incremental ? drill.L : 1; // no need to draw absolute repeats(?) setEndP(drill.Values, AxisFlags.XY); isRelative = false; if (!canned) { // canned = true; if (action.End.Z < r) { double[] start = new double[] { action.Start.X, action.Start.Y, r }; setEndP(start, AxisFlags.Z); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, start, AxisFlags.Z); yield return(action); action.Start = action.End; } } double[] values = new double[] { action.End.X, action.End.Y, action.End.Z }; setEndP(values, AxisFlags.X | AxisFlags.Y); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.XY); yield return(action); action.Start = action.End; do { values[2] = z; setEndP(values, AxisFlags.Z); action.Token = new GCLinearMotion(Commands.G1, token.LineNumber, values, AxisFlags.Z); yield return(action); action.Start = action.End; values[2] = r; setEndP(values, AxisFlags.Z); action.IsRetract = true; action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.Z); yield return(action); action.Start = action.End; action.IsRetract = false; if (repeats > 1) { values[0] += drill.X; values[1] += drill.Y; setEndP(values, AxisFlags.X | AxisFlags.Y); action.Token = new GCLinearMotion(Commands.G0, token.LineNumber, values, AxisFlags.XY); yield return(action); action.Start = action.End; } } while (--repeats > 0); isRelative = wasRelative; action.Token = token; } break; // G90.1, G91.1: Arc Distance Mode case Commands.G90: // Absolute case Commands.G91: // Incremental isRelative = (token as GCDistanceMode).DistanceMode == DistanceMode.Incremental; break; // G92: Coordinate System Offset case Commands.G92: { var cs = token as GCCoordinateSystem; foreach (int i in cs.AxisFlags.ToIndices()) { origin[i] = g92.Values[i] = cs.Values[i]; } } break; // G92.1: Reset G92 Offsets - Clear Parameters case Commands.G92_1: { for (int i = 0; i < origin.Length; i++) { origin[i] = g92.Values[i] = 0d; } } break; // G92.1: Reset G92 Offsets - Keep Parameters case Commands.G92_2: { for (int i = 0; i < origin.Length; i++) { origin[i] = 0d; } } break; //M3, M4, M5: Spindle Control case Commands.M3: // CW case Commands.M4: // CCW case Commands.M5: // Off SpindleState = (token as GCSpindleState).SpindleState; break; // M7, M8, M9: Coolant Control case Commands.M7: // Mist case Commands.M8: // Flood case Commands.M9: // Off CoolantState = (token as GCCoolantState).CoolantState; break; case Commands.Feedrate: Feedrate = (token as GCFeedrate).Feedrate; break; case Commands.SpindleRPM: _rpm = (token as GCSpindleRPM).SpindleRPM; break; // M61, T: Set Current Tool case Commands.M61: case Commands.ToolSelect: Tool = (token as GCToolSelect).Tool; break; } if (action.Token.Command != Commands.Undefined) { yield return(action); } action.Start = action.End; } }