protected override void SolveInstance(IGH_DataAccess DA)
{
// Set up solution variables
List <Kernal.Instruction> instructions = new List <Kernal.Instruction>();
DateTime now = DateTime.Now;
Robot c_Robot = null;
// Load Inputs
if (!DA.GetData(0, ref c_Robot))
{
return;
}
if (!DA.GetDataList("Instructions", instructions))
{
return;
}
if (!run | newData)
{
//Set Up Tool path
toolpath = new Toolpath(instructions, c_Robot);
this.Message = (toolpath.IsValid) ? "No Errors detected" : "Errors";
strat = DateTime.Now;
if (!toolpath.IsValid)
{
AddRuntimeMessage(GH_RuntimeMessageLevel.Error, "Invalid Toolpath");
return;
}
c_Pose = toolpath.StartPose;
newData = false;
}
// Out put a full error log
if (toolpath != null && fullprogramCheck.Checked && !run)
{
if (!DA.SetDataList("Full Error Log", toolpath.ErrorLog))
{
return;
}
if (!DA.SetDataList("Error Positions", toolpath.ErrorPositions))
{
return;
}
}
if (toolpath != null && run)
{
if (c_Pose == null)
{
c_Pose = toolpath.GetPose(DateTime.Now - TimeSpan.FromSeconds(0.1) - strat);
}
var pose = toolpath.GetPose(DateTime.Now - strat);
if (!pose.IsValid)
{
c_Pose.Colors = pose.Colors;
}
else
{
c_Pose = pose;
}
if ((DateTime.Now - strat - TimeSpan.FromSeconds(1)) > toolpath.duration)
{
run = false;
ExpireSolution(true);
}
}
else if (timelineOption.Checked && !run)
{
double tValue = 0;
if (!DA.GetData("*Timeline", ref tValue))
{
return;
}
c_Pose = toolpath.GetPose(tValue);
}
// DA.SetData("Target", targets[0]);
if (c_Pose != null)
{
// Handle errors
void SetLogMessages(Robot.Pose Poses, List <string> Log)
{
if (Poses.OverHeadSig)
{
Log.Add("Close to overhead singularity.");
}
if (Poses.WristSing)
{
Log.Add("Close to wrist singularity.");
}
if (Poses.OutOfReach)
{
Log.Add("Target out of range.");
}
if (Poses.OutOfRoation)
{
Log.Add("Joint out of range.");
}
}
List <string> log = new List <string>();
SetLogMessages(c_Pose, log);
// Set output
if (flangeCheck.Checked)
{
DA.SetData("Flange", c_Pose.Flange);
}
if (anglesCheck.Checked)
{
DA.SetDataList("Angles", c_Pose.Angles);
}
if (speedCheck.Checked)
{
DA.SetData("Speed", c_Pose.Speed.TranslationSpeed);
}
if (motionCheck.Checked)
{
DA.SetData("Motion", c_Pose.Target.Method.ToString());
}
if (externalCheck.Checked)
{
DA.SetData("External", (double)c_Pose.Target.ExtRot);
}
//if (showExternal) DA.SetData("External", (double)c_Pose.Target.ExtLin);
DA.SetDataList("Log", log);
// Update and display data
c_Pose.GetBoundingBox(Transform.Identity);
if (run)
{
ExpireSolution(true);
}
}
}
protected override void SolveInstance(IGH_DataAccess DA)
{
Plane m_baseplane = Plane.WorldXY;
DA.GetData("Baseplane", ref m_baseplane);
Plane m_workplane = Plane.WorldXY;
DA.GetData("Workplane", ref m_workplane);
Point3d m_point = Point3d.Origin;
DA.GetData("Point", ref m_point);
MachineTool m_machine_tool = null;
DA.GetData("MachineTool", ref m_machine_tool);
Toolpath m_toolpath = null;
DA.GetData("Toolpath", ref m_toolpath);
Mesh m_stock = null;
DA.GetData("Stock mesh", ref m_stock);
bool m_reset = false;
DA.GetData("Reset", ref m_reset);
Toolpath tpViz = null; int tpVizN = 0;
if (m_toolpath != null)
{
tpViz = m_toolpath.Duplicate();
}
// Transform the workpiece and TCP to the G54 work offset
Transform G54_xform = Transform.PlaneToPlane(Plane.WorldXY, m_workplane);
m_stock.Transform(G54_xform);
m_point.Transform(G54_xform);
if (tpViz != null)
{
tpVizN = XFORM(tpViz, G54_xform);
}
// Machine coordinates
var coords = new double[] {
m_point.X,
m_point.Y,
m_point.Z + m_machine_tool.Length
};
/*
* bool is_good;
* for (int i = 0; i < 3; ++i)
* if (!m_limits[i].IncludesParameter(coords[i]))
* {
* is_good = false;
* }
* is_good = true;
*/
if (!IsInMachineLimits(coords))
{
this.Message = "Out of bounds!";
}
else
{
this.Message = "";
}
if (m_reset)
{
m_machine_meshes = new List <Mesh>();
DA.GetDataList("Machine meshes", m_machine_meshes);
// Hard-code machine part transformations (could also just move the meshes in place)
machine_part_xforms[0] = Transform.Translation(new Vector3d(0, 0, -506));
machine_part_xforms[1] = Transform.Translation(new Vector3d(-508, -254, -506));
machine_part_xforms[2] = Transform.Translation(new Vector3d(-508, -254, -506));
machine_part_xforms[3] = Transform.Translation(new Vector3d(0, 0, -101.6));
machine_part_xforms[4] = Transform.Translation(new Vector3d(0, 0, 0));
}
// Hard-code meshes
meshes[0] = // chassis
m_machine_meshes[0].DuplicateMesh();
meshes[2] = // bed (moves in XY)
m_machine_meshes[1].DuplicateMesh();
meshes[3] = // tower
m_machine_meshes[2].DuplicateMesh();
meshes[4] = //new Mesh();
m_machine_meshes[3].DuplicateMesh(); // tool
meshes[4].Append(
Mesh.CreateFromCylinder(new Cylinder(new Circle(
new Plane(
new Point3d(0, 0, 0),
-Vector3d.XAxis, Vector3d.YAxis),
m_machine_tool.Diameter / 2), m_machine_tool.Length), 4, 24));
// Pre-move all the machine parts in place
for (int i = 0; i < machine_part_xforms.Length; ++i)
{
if (meshes[i] == null)
{
continue;
}
meshes[i].Transform(machine_part_xforms[i]);
}
// Global transform
xforms[0] = Transform.PlaneToPlane(Plane.WorldXY, m_baseplane);
// Convert point to transforms
xforms[1] = Transform.Translation(new Vector3d(-m_point.X, 0, 0));
xforms[2] = Transform.Translation(new Vector3d(0, -m_point.Y, 0));
xforms[3] = Transform.Translation(new Vector3d(0, 0, m_point.Z + m_machine_tool.Length));
// Transform machine parts
for (int i = meshes.Length - 1; i >= 0; --i)
{
if (meshes[i] == null)
{
continue;
}
for (int j = 0; j < axis_relations[i].Length; ++j)
{
meshes[i].Transform(xforms[axis_relations[i][j]]);
}
}
// Transform workpiece
Transform total = G54_xform;
for (int i = wp_relations.Length - 1; i >= 0; --i)
{
m_stock.Transform(xforms[wp_relations[i]]);
m_workplane.Transform(xforms[wp_relations[i]]);
if (tpViz != null)
{
tpVizN = XFORM(tpViz, xforms[wp_relations[i]]);
}
total = Transform.Multiply(total, xforms[wp_relations[i]]);
}
DA.SetData("Path", new Polyline(tpViz.Paths.SelectMany(x => x.Select(y => y.Plane.Origin))));
DA.SetData("Toolpath", new GH_Toolpath(tpViz));
DA.SetData("Work offset", m_workplane);
DA.SetData("Stock mesh", m_stock);
DA.SetDataList("Machine meshes", meshes);
}
/// <summary>
/// This is the method that actually does the work.
/// </summary>
/// <param name="DA">The DA object is used to retrieve from inputs and store in outputs.</param>
protected override void SolveInstance(IGH_DataAccess DA)
{
List <object> iToolpaths = new List <object>();
object iTool = null;
Plane iSafe = Plane.WorldXY;
double iRapidZ = 20, iSafeZ = 10;
bool iVR = true;
string name = "Toolpath";
bool flipWrist = false;
DA.GetData("Name", ref name);
DA.GetDataList("Toolpaths", iToolpaths);
DA.GetData("Machine Tool", ref iTool);
DA.GetData("Safety", ref iSafe);
DA.GetData("RapidZ", ref iRapidZ);
DA.GetData("SafeZ", ref iSafeZ);
DA.GetData("VertRetract", ref iVR);
DA.GetData("FlipWrist", ref flipWrist);
this.Message = flipWrist.ToString();
Toolpath tp = new Toolpath();
tp.Name = name;
tp.FlipWrist = flipWrist;
// Cast tool
MachineTool mt;
if (iTool is GH_MachineTool)
{
mt = (iTool as GH_MachineTool).Value;
}
else
{
mt = iTool as MachineTool;
}
if (mt == null)
{
this.AddRuntimeMessage(GH_RuntimeMessageLevel.Error, "Machine tool could not be cast.");
return;
}
tp.Tool = mt;
for (int i = 0; i < iToolpaths.Count; ++i)
{
Path poly;
if (iToolpaths[i] is Path)
{
poly = iToolpaths[i] as Path;
}
else if (iToolpaths[i] is GH_tasPath)
{
poly = (iToolpaths[i] as GH_tasPath).Value;
}
else
{
this.AddRuntimeMessage(GH_RuntimeMessageLevel.Error, "Error in Path wrangling.");
continue;
}
tp.Paths.Add(poly.Select(x => new Waypoint(x, (int)WaypointType.FEED)).ToList());
}
tp.PlaneRetractVertical = iVR;
tp.RapidZ = iRapidZ;
tp.SafeZ = iSafeZ;
tp.Safety = iSafe;
DA.SetData("Toolpath", new GH_Toolpath(tp));
}
public override object Compute()
{
if (Paths.Count < 1)
{
this.Errors.Add("No paths to process...");
return(null);
}
//List<string> Tools = new List<string>();
for (int i = 0; i < Paths.Count; ++i)
{
if (!Tools.ContainsKey(Paths[i].Tool.Name))
{
this.Errors.Add($"Tool '{Paths[i].Tool.Name}' not found in post-processor tool library.");
continue;
}
}
Axes = new List <AxisValues>();
Program = new List <string>();
Errors = new List <string>();
//bool HighSpeed = true;
// Create headers
Program.Add("%");
Program.Add("O0001");
BoundingBox = BoundingBox.Empty;
if (StockModel != null)
{
BoundingBox = StockModel.GetBoundingBox(true);
}
CreateHeader();
Program.Add("");
Program.Add($"{PreComment} * * * * * WORK PLANE * * * * * {PostComment}");
Program.Add($"G{GWorkOffset} X{WorkOffset.X} Y{WorkOffset.Y} Z{WorkOffset.Z}");
/* Old CMS defaults and settings */
/*
* Program.Add($"{PreComment} * * * * * VARIABLES * * * * * {PostComment}");
* Program.Add($"#560 = {GWorkOffset} {PreComment}ZERO POINT{PostComment}");
* Program.Add($"#561 = {WorkOffset.X} {PreComment}OFFSET PROGRAM I X{PostComment}");
* Program.Add($"#562 = {WorkOffset.Y} {PreComment}OFFSET PROGRAM I Y{PostComment}");
* Program.Add($"#563 = {WorkOffset.Z} {PreComment}OFFSET PROGRAM I Z{PostComment}");
*
* Program.Add($"#564 = {MaterialThickness} {PreComment}EMNE TYKKELSE{PostComment}");
* Program.Add($"#565 = {SikkerZ} {PreComment}SIKKER Z)");
* Program.Add($"#566 = {SikkerZPlanskifte} {PreComment}SIKKER Z VED PLANSKIFTE{PostComment}");
* Program.Add($"#563 = #563 + #564");
* Program.Add($"#569 = {60}");
*
* Program.Add("");
*/
Program.Add("");
Program.Add($"{PreComment} * * * * * START * * * * * {PostComment}");
// Init gcode
Program.Add("G90 G40 G80 G49 G69");
// Reset offsets to 0
Program.Add("G92.1 X0 Y0 Z0 B0 C0");
// CMS pressure clamping closing (unnecessary)
//Program.Add("M25");
// Set to mm (G20 is inches)
Program.Add("G21");
// Go home. G0 = rapid, G53 = move in absolute coords
Program.Add("G0 G53 Z0");
Program.Add("G0 B0 C0");
//Program.Add("G#560");
// Set work offset
//Program.Add("G#560 X#561 Y#562 Z#563");
Program.Add($"G{GWorkOffset}");
// Working variables
int G_VALUE = -1;
int flags = 0;
bool write_feedrate = true;
// Initialize coordinates
double[] coords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
coords[i] = double.MaxValue;
}
double[] pCoords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
pCoords[i] = double.MaxValue;
}
int currentFeedrate = 0;
int tempFeedrate = int.MaxValue;
int prevTool = -1;
for (int i = 0; i < Paths.Count; ++i)
{
bool flip = false;
if (i < m_flipList.Count)
{
flip = m_flipList[i];
}
Toolpath TP = Paths[i];
// Add toolpath info
// TODO: Add support for tool indexing to the whole thing
Program.Add("");
Program.Add("");
Program.Add($"{PreComment} * * * * * PATH {i:D2} * * * * * {PostComment}");
Program.Add($"{PreComment} Operation : {TP.Name} {PostComment}");
Program.Add($"{PreComment} Tool no. : {Tools[TP.Tool.Name].Number} {PostComment}");
Program.Add($"{PreComment} Tool des. : {Tools[TP.Tool.Name].Name} {PostComment}");
Program.Add($"{PreComment} Tool dia. : {Tools[TP.Tool.Name].Diameter} {PostComment}");
// Tool change
Program.Add($"M6 T{Tools[TP.Tool.Name].Number}");
if (TP.Tool.Number != prevTool)
{
// Start spindle
Program.Add($"M3 S{Tools[TP.Tool.Name].SpindleSpeed}");
}
prevTool = TP.Tool.Number;
// Not sure what this does...
/*
* Program.Add("#567 = #2255+135.0");
* Program.Add("#568 = 0 + SQRT[#567*#567+625]+#566-135");
* Program.Add("G#560");
*
*/
Program.Add("");
//Program.Add("G0 G53 Z0 B-90");
if (HighSpeed)
{
Program.Add("G5.1 Q1");
}
Program.Add($"G43.4 H{Tools[TP.Tool.Name].OffsetNumber}");
// If toolpath is planar, lock B and C axes
if (TP.IsPlanar)
{
Program.Add($"M32 M34");
}
// Move to first waypoint
//Waypoint prev = TP.Paths[0][0];
//if (prev.Type != (int)WaypointType.RAPID)
// throw new Exception("First waypoint must be rapid. Check code.");
Waypoint prev = new Waypoint(TP.Paths[0][0]);
prev.Type = (int)WaypointType.RAPID;
// Calculate B and C values
//PlaneToCoords(prev.Plane, ref pCoords);
//if (flip)
// FlipWrist(ref pCoords);
PlaneToCoords(prev.Plane, ref pCoords);
if (TP.FlipWrist)
{
FlipWrist(ref pCoords);
}
// Vector3d axisFirst = prev.Plane.ZAxis;
//axisFirst.Unitize();
//prevB = Rhino.RhinoMath.ToDegrees(Math.Acos(axisFirst * Vector3d.ZAxis));
//prevC = Rhino.RhinoMath.ToDegrees(Math.Atan2(axisFirst.Y, axisFirst.X));
//Program.Add($"G{(int)prev.Type} X{prev.Plane.Origin.X:F3} Y{prev.Plane.Origin.Y:F3} Z{prev.Plane.Origin.Z:F3} B{prevB:F3} C{prevC:F3}");
//Program.Add($"G0 X{prev.Plane.Origin.X:F3} Y{prev.Plane.Origin.Y:F3} B{prevB:F3} C{prevC:F3} Z#568");
Program.Add($"G0 X{pCoords[0]:F3} Y{pCoords[1]:F3}");
Program.Add($"G0 B{pCoords[3]:F3} C{pCoords[4]:F3}");
Program.Add($"G0 Z{pCoords[2]:F3}");
double diff;
// Go through waypoints
for (int j = 0; j < TP.Paths.Count; ++j)
{
// Parse sub-paths
WPath Subpath = TP.Paths[j];
for (int k = 0; k < Subpath.Count; ++k)
{
write_feedrate = false;
flags = 0;
Waypoint wp = Subpath[k];
PlaneToCoords(wp.Plane, ref coords);
//if (flip)
// FlipWrist(ref coords);
if (TP.FlipWrist)
{
FlipWrist(ref coords);
}
// Deal with abrupt 180 to -180 switches
diff = coords[4] - pCoords[4];
if (diff > 270)
{
coords[4] -= 360.0;
}
if (diff < -270)
{
coords[4] += 360.0;
}
// Check limits
//if (!IsInMachineLimits(coords))
// Errors.Add($"Waypoint outside of machine limits: toolpath {i} subpath {j} waypoint {k} : {wp}, {coords[3]}, {coords[4]}");
if (!this.CheckAbsoluteLimits(wp.Plane, TP.Tool))
{
var temp_pt = GetMachinePosition(wp.Plane, TP.Tool);
Errors.Add($"Target {k} in toolpath {j} ({TP.Name}) out of bounds: {temp_pt.X:0.00} {temp_pt.Y:0.00} {temp_pt.Z:0.00}");
}
// Compose line
List <string> Line = new List <string>();
#region Parse movement (G code)
if (wp.Type != prev.Type || AlwaysWriteGCode)
{
flags = flags | 1;
if (wp.IsRapid())
{
G_VALUE = 0;
write_feedrate = false;
tempFeedrate = int.MaxValue;
}
else if (wp.IsArc())
{
write_feedrate = true;
if (wp.IsClockwise())
{
G_VALUE = 3;
}
else
{
G_VALUE = 2;
}
}
else
{
G_VALUE = 1;
write_feedrate = true;
}
}
#endregion
#region Parse movement on axes
for (int l = 0; l < m_dof; ++l)
{
if (Math.Abs(coords[l] - pCoords[l]) > 0.001)
{
flags = flags | (1 << (l + 1));
}
}
#endregion
#region Write feedrate if different
// If Plunge move, set current feedrate to PlungeRate
if (write_feedrate)
{
if (wp.IsPlunge())
{
tempFeedrate = Tools[TP.Tool.Name].PlungeRate;
}
else
{
tempFeedrate = Tools[TP.Tool.Name].FeedRate;
}
// If new feedrate is different from old one, write F value
write_feedrate = tempFeedrate != currentFeedrate ? true : false;
}
currentFeedrate = tempFeedrate;
#endregion
// If it is an arc move, then write I J K values
if (wp.IsArc())
{
flags = flags | (1 << (m_dof + 1));
}
if (write_feedrate)
{
flags = flags | (1 << (m_dof + 2));
}
// If there is no motion, skip this waypoint
if ((flags & m_NO_MOTION) < 1)
{
continue;
}
Axes.Add(new AxisValues(coords[0], coords[1], coords[2], coords[3], coords[4], currentFeedrate));
#region Construct NC code
if ((flags & 1) > 0)
{
Line.Add($"G{G_VALUE:00}");
}
for (int l = 0; l < m_dof; ++l)
{
if ((flags & (1 << (1 + l))) > 0)
{
Line.Add($"{m_axis_id[l]}{coords[l]:F3}");
}
}
if ((flags & (1 << (m_dof + 1))) > 0)
{
Line.Add($"R{wp.Radius:F3}");
}
if ((flags & (1 << (m_dof + 2))) > 0)
{
Line.Add($"F{currentFeedrate}");
}
#endregion
// Add line to program
Program.Add(string.Join(" ", Line) + ";");
prev = wp;
Array.Copy(coords, pCoords, coords.Length);
}
}
Program.Add("");
// Stop spindle
//Program.Add("M5");
// TODO: Find out what these G codes do
//Program.Add("G49 G53 G69");
Program.Add("G49");
Program.Add("G69");
if (HighSpeed)
{
Program.Add("G5.1 Q0");
}
// TODO: Add another home position here?
// If toolpath is planar, unlock B and C axes
if (TP.IsPlanar)
{
Program.Add($"M31 M33");
}
//Program.Add("G0 G53 Z0");
//Program.Add("G0 G53 B-90");
Program.Add("");
}
// Return home
// TODO: Look at example, find out if G53 is global coords
// and add if necessary
//Program.Add("G53");
Program.Add("G0 G53 Z0");
Program.Add("M5");
//if (prevB >= 0)
// Program.Add("G0 G53 B90");
//else if (prevB < 0)
// Program.Add("G0 G53 B-90");
// This is the funny dance
//Program.Add("G0 G53 B-90");
//Program.Add("G0 G53 C0");
Program.Add("G0 G53 B0 C0");
Program.Add("G92.1 X0 Y0 Z0 B0 C0");
Program.Add("G0 G53 Y0");
Program.Add("G0 G53 X0");
//Program.Add("G0 G53 X-2500");
//Program.Add("G0 G53 B0");
//Program.Add("G0 G53 X-2600");
Program.Add($"{PreComment} * * * * * END * * * * * {PostComment}");
//Program.Add("M7"); // This should be to return the tool, but check
//Program.Add("");
Program.Add("M99");
Program.Add("%");
return(Program);
}
public override object Compute()
{
if (Paths.Count < 1)
{
this.Errors.Add("No paths to process...");
return(null);
}
for (int i = 0; i < Paths.Count; ++i)
{
if (!Tools.ContainsKey(Paths[i].Tool.Name))
{
this.Errors.Add(string.Format("Tool '{0}' not found in post-processor tool library.", Paths[i].Tool.Name));
continue;
}
}
Program = new List <string>();
Errors = new List <string>();
MachineTool ActiveTool;
BoundingBox = BoundingBox.Empty;
if (StockModel != null)
{
BoundingBox = StockModel.GetBoundingBox(true);
}
// Working variables
int G_VALUE = -1;
int flags = 0;
bool write_feedrate = true;
// Initialize coordinates
double[] coords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
coords[i] = double.MaxValue;
}
double[] pCoords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
pCoords[i] = double.MaxValue;
}
int currentFeedrate = 0;
int tempFeedrate = int.MaxValue;
EOL = " ";
Program.Add("%");
//Program.Add($"O01001 ({Name})"); // Program number / name
CreateHeader();
//Program.Add("G00 G17 G40 G49 G80 G90 G98;"); // Safety line
//Program.Add("G00 G53 Z0;"); // Return to machine zero
Program.Add("G90"); // Return to machine zero
Program.Add("G64"); // Return to machine zero
Program.Add("M7"); // Return to machine zero
Program.Add("M8"); // Return to machine zero
// Loop through Toolpaths
for (int i = 0; i < Paths.Count; ++i)
{
Toolpath TP = Paths[i];
ActiveTool = Tools[TP.Tool.Name];
Program.Add($"{PreComment}----------------------------------------------------------------{PostComment}{EOL}");
Program.Add($"{PreComment} START Toolpath: {TP.Name} {PostComment}{EOL}");
Program.Add($"{PreComment} Tool: {ActiveTool.Name} Diameter: {ActiveTool.Diameter} {PostComment}{EOL}");
Program.Add($"{PreComment}----------------------------------------------------------------{PostComment}{EOL}");
// Tool change
// TODO: Change so that it only changes the tool if necessary, though the machine should ignore this anyway
Program.Add($"T{Tools[TP.Tool.Name].Number}{EOL}");
// Move to first waypoint
Waypoint prev = new Waypoint(TP.Paths[0][0]);
prev.Type = (int)WaypointType.RAPID;
Program.Add($"S{TP.Tool.SpindleSpeed}");
Program.Add("M3");
//Program.Add($"G00 G90 G21 G54 X{prev.Plane.Origin.X:F3} Y{prev.Plane.Origin.Y:F3} S{TP.Tool.SpindleSpeed} M03{EOL}");
//Program.Add($"G43 H{Tools[TP.Tool.Name].OffsetNumber:00} M08{EOL}");
// Loop through subpaths
for (int j = 0; j < TP.Paths.Count; ++j)
{
WPath Subpath = TP.Paths[j];
// Loop through individual waypoints
for (int k = 0; k < Subpath.Count; ++k)
{
// Reset working variables
write_feedrate = false;
flags = 0;
Waypoint wp = Subpath[k];
// Convert waypoint targets to axis coordinates
PlaneToCoords(wp.Plane, ref coords);
PlaneToCoords(prev.Plane, ref pCoords);
// Check limits
if (!IsInMachineLimits(coords))
{
Errors.Add($"Waypoint outside of machine limits: toolpath {i} subpath {j} waypoint {k} : {wp}");
}
// Compose NC line
List <string> Line = new List <string>();
#region Parse movement (G code)
if (wp.Type != prev.Type || AlwaysWriteGCode || true)
{
flags = flags | 1;
if (wp.IsRapid())
{
G_VALUE = 0;
write_feedrate = false;
}
else if (wp.IsArc())
{
write_feedrate = true;
if (wp.IsClockwise())
{
G_VALUE = 3;
}
else
{
G_VALUE = 2;
}
}
else
{
G_VALUE = 1;
if (prev.Type == (int)WaypointType.RAPID)
{
write_feedrate = true;
}
}
}
#endregion
#region Write feedrate if different
// If Plunge move, set current feedrate to PlungeRate
if (wp.IsPlunge())
{
tempFeedrate = Tools[TP.Tool.Name].PlungeRate;
}
else
{
tempFeedrate = Tools[TP.Tool.Name].FeedRate;
}
// If new feedrate is different from old one, write F value
if (tempFeedrate != currentFeedrate)
{
write_feedrate = true;
}
currentFeedrate = tempFeedrate;
#endregion
#region Parse movement on axes
for (int l = 0; l < m_dof; ++l)
{
if (Math.Abs(coords[l] - pCoords[l]) > 0.00001)
{
flags = flags | (1 << (l + 1));
}
}
#endregion
// If it is an arc move, then write I J K values
if (wp.IsArc())
{
flags = flags | (1 << m_dof + 1);
}
if (write_feedrate)
{
flags = flags | (1 << m_dof + 2);
}
// If there is no motion, skip this waypoint
if ((flags & m_NO_MOTION) < 1)
{
continue;
}
#region Construct NC code
if ((flags & 1) > 0)
{
Line.Add($"G{G_VALUE:0}");
}
for (int l = 0; l < m_dof; ++l)
{
//if ((flags & (1 << 1 + l)) > 0)
Line.Add($"{m_axis_id[l]}{coords[l]:F3}");
}
if ((flags & (1 << m_dof + 1)) > 0)
{
Line.Add($"R{wp.Radius:F3}");
}
if ((flags & (1 << m_dof + 2)) > 0)
{
Line.Add($"F{currentFeedrate}");
}
#endregion
// Add line to program
Program.Add(string.Join(" ", Line) + EOL);
// Update previous waypoint
prev = new Waypoint(wp);
}
}
//Program.Add($"{PreComment}{PostComment}{EOL}");
//Program.Add($"G53 G49 G0 Z0.{EOL}");
//Program.Add("G53 X0. Y0.");
//Program.Add("G0 Z12");
Program.Add($"{PreComment} ---- END Toolpath: {TP.Name} ---- {PostComment}{EOL}");
}
//Program.Add($"G00 Z10.");
Program.Add($"{PreComment} End of program {PostComment}{EOL}");
Program.Add($"M9");
Program.Add($"M5{EOL}"); // Spindle stop
Program.Add($"M30{EOL}");
//Program.Add("%");
return(Program);
}
public override object Compute()
{
if (Paths.Count < 1)
{
this.Errors.Add("No paths to process...");
return(null);
}
for (int i = 0; i < Paths.Count; ++i)
{
if (!Tools.ContainsKey(Paths[i].Tool.Name))
{
this.Errors.Add(string.Format("Tool '{0}' not found in post-processor tool library.", Paths[i].Tool.Name));
continue;
}
}
List <string> Program = new List <string>();
Errors = new List <string>();
BoundingBox bbox = BoundingBox.Unset;
if (StockModel != null)
{
bbox = StockModel.GetBoundingBox(true);
}
// Create headers
Program.Add("O01001;"); // Program number / name
Program.Add($"{PreComment}{PostComment};");
Program.Add($"{PreComment} Revision : 1 {PostComment};");
Program.Add($"{PreComment}{PostComment}");
Program.Add($"{PreComment} File name : {Name} {PostComment};");
Program.Add($"{PreComment} Programmed by : {Author} {PostComment};");
Program.Add($"{PreComment} Date : {Date} {PostComment};");
Program.Add($"{PreComment} Program length : {ProgramTime} {PostComment}");
Program.Add($"{PreComment} Bounds min. : {bbox.Min.X} {bbox.Min.Y} {bbox.Min.Z} {PostComment};");
Program.Add($"{PreComment} Bounds max. : {bbox.Max.X} {bbox.Max.Y} {bbox.Max.Z} {PostComment};");
Program.Add($"{PreComment}{PostComment};");
Program.Add($"{PreComment}Tool # Offset # Name Diameter Length {PostComment};");
foreach (var d in Tools)
{
MachineTool mt = d.Value;
Program.Add($"{PreComment} {mt.Number} {mt.OffsetNumber} {mt.Name} {mt.Diameter:0.0} {mt.Length:0.000} {PostComment};");
}
Program.Add($"{PreComment}{PostComment};");
Program.Add("G0 Z12");
// Working variables
int G_VALUE = -1;
int flags = 0;
bool write_feedrate = true;
// Initialize coordinates
double[] coords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
coords[i] = double.MaxValue;
}
double[] pCoords = new double[DOF];
for (int i = 0; i < DOF; ++i)
{
pCoords[i] = double.MaxValue;
}
int currentFeedrate = 0;
int tempFeedrate = int.MaxValue;
for (int i = 0; i < Paths.Count; ++i)
{
Toolpath TP = Paths[i];
Program.Add("");
// Tool change
Program.Add($"M6 T{Tools[TP.Tool.Name].Number}");
// Move to first waypoint
Waypoint prev = new Waypoint(TP.Paths[0][0]);
prev.Type = (int)WaypointType.RAPID;
//if (prev.Type != (int)WaypointType.RAPID)
// throw new Exception("First waypoint must be rapid. Check code.");
Program.Add($"G0 X{prev.Plane.Origin.X:F3} Y{prev.Plane.Origin.Y:F3}");
// Go through waypoints
for (int j = 0; j < TP.Paths.Count; ++j)
{
// Parse sub-paths
WPath Subpath = TP.Paths[j];
for (int k = 0; k < Subpath.Count; ++k)
{
write_feedrate = false;
flags = 0;
Waypoint wp = Subpath[k];
PlaneToCoords(wp.Plane, ref coords);
// Check limits
if (!IsInMachineLimits(coords))
{
Errors.Add($"Waypoint outside of machine limits: toolpath {i} subpath {j} waypoint {k} : {wp}");
}
// Compose line
List <string> Line = new List <string>();
if (wp.Type != prev.Type || AlwaysWriteGCode)
{
flags = flags | 1;
if ((wp.Type & 1) == 1)
{
G_VALUE = 0;
}
else if ((wp.Type & 4) == 1)
{
if ((wp.Type & 12) == 1)
{
G_VALUE = 3;
}
else
{
G_VALUE = 2;
}
}
else
{
G_VALUE = 1;
write_feedrate = true;
}
}
#region Parse movement on axes
for (int l = 0; l < m_dof; ++l)
{
if (Math.Abs(coords[l] - pCoords[l]) > 0.00001)
{
flags = flags | (1 << (l + 1));
}
}
#endregion
// If Plunge move, set current feedrate to PlungeRate
if (wp.IsPlunge())
{
tempFeedrate = Tools[TP.Tool.Name].PlungeRate;
}
else
{
tempFeedrate = Tools[TP.Tool.Name].FeedRate;
}
// If new feedrate is different from old one, write F value
if (tempFeedrate == currentFeedrate)
{
write_feedrate = false;
}
currentFeedrate = tempFeedrate;
// If it is an arc move, then write R value
if (wp.IsArc())
{
flags = flags | (1 << 4);
}
if (write_feedrate)
{
flags = flags | (1 << 5);
}
// If there is no motion, skip this waypoint
if ((flags & m_NO_MOTION) < 1)
{
continue;
}
#region Construct NC code
if ((flags & 1) > 0)
{
Line.Add($"G{G_VALUE:00}");
}
for (int l = 0; l < m_dof; ++l)
{
if ((flags & (1 << 1 + l)) > 0)
{
Line.Add($"{m_axis_id[l]}{coords[l]:F3}");
}
}
if ((flags & (1 << m_dof + 1)) > 0)
{
Line.Add($"R{wp.Radius:F3}");
}
if ((flags & (1 << m_dof + 2)) > 0)
{
Line.Add($"F{currentFeedrate}");
}
#endregion
// Add line to program
Program.Add(string.Join(" ", Line));
// Update previous waypoint
prev = new Waypoint(wp);
}
}
Program.Add("");
Program.Add("G0 Z12");
Program.Add("");
}
Program.Add("G0 Z0");
Program.Add("G0 X0 Y0");
Program.Add("M30");
return(Program);
}
public void BuildSimulation(List <Toolpath> toolpaths)
{
m_wp = new List <Waypoint>();
m_times = new List <double>();
m_feeds = new List <double>();
m_toolpaths = new List <Toolpath>();
m_toolpath_times = new List <double>();
Waypoint cTarget, cPrev;
cTime = 0;
cPrev = toolpaths.First().Paths.First().First();
//m_times.Add(0);
int feed_index = 0;
for (int i = 0; i < toolpaths.Count; ++i)
{
Toolpath tp = toolpaths[i];
m_toolpaths.Add(tp);
m_toolpath_times.Add(cTime);
for (int j = 0; j < tp.Paths.Count; ++j)
{
for (int k = 0; k < tp.Paths[j].Count; ++k)
{
cTarget = tp.Paths[j][k];
m_wp.Add(cTarget);
if (cTarget.IsFeed())
{
m_feeds.Add(tp.Tool.FeedRate);
}
else if (cTarget.IsPlunge())
{
m_feeds.Add(tp.Tool.PlungeRate);
}
else if (cTarget.IsRapid())
{
m_feeds.Add(tp.Tool.FeedRate * 2);
}
else
{
throw new Exception("F**k!");
}
cTime += cTarget.Plane.Origin.DistanceTo(cPrev.Plane.Origin) / m_feeds[feed_index];
feed_index++;
m_times.Add(cTime);
cPrev = cTarget;
}
}
}
foreach (double d in m_toolpath_times)
{
}
if (m_wp.Count < 1)
{
throw new Exception("No waypoints found!");
}
}
protected override void SolveInstance(IGH_DataAccess DA)
{
DA.GetData("Calculate", ref this._calc);
if (!this._calc)
{
DA.SetDataList("Targets", this._targets);
DA.SetData("Path", this._path);
//DA.SetDataList("Planes", this._planes);
DA.SetData("debug", this._debug);
return;
}
this._debug = "";
this._path = null;
this._targets = null;
this._planes = null;
Plane frame_t = new Plane();
this._toolpaths = new List <Polyline>();
// collect inputs
DA.GetData("Workplane", ref this._workplane);
DA.GetData("External axes", ref this._extern);
DA.GetData("Configuration", ref this._config);
if (!DA.GetData("Frame", ref frame_t))
{
_frame = Frame.Default;
}
//_frame = new Frame(Plane.WorldXY, "tasFrame");
else
{
_frame = new Frame(frame_t, -1, -1, "tasFrame");
}
Robots.Grasshopper.GH_Tool toolGH = null;
DA.GetData("Tool", ref toolGH);
this._tool = toolGH?.Value;
this._debug += toolGH.ToString() + "\n";
this._debug += toolGH.GetType().ToString() + "\n";
if (this._tool == null)
{
this._debug += "Tool conversion failed...\n";
}
else
{
this._debug += this._tool.ToString() + "\n";
this._debug += this._tool.GetType().ToString() + "\n";
}
// gather and convert, if necessary, input curves
List <Curve> in_crvs = new List <Curve>();
if (!DA.GetDataList("Toolpaths", in_crvs))
{
return;
}
this._toolpaths = tasUtility.CurvesToPolylines(in_crvs, 1.0);
this._is_drive_surface = DA.GetData("DriveSurface", ref this._drive_surface);
DA.GetData("Safe Z", ref this._safe_z);
DA.GetData("Rapid Z", ref this._rapid_z);
DA.GetData("Feed rate", ref this._feedrate);
DA.GetData("Rapid rate", ref this._rapidrate);
DA.GetData("Ramp height", ref this._ramp_height);
DA.GetData("Tool twist", ref this._tooltwist);
// /collect inputs
// set up speeds
Speed sp_feed = new Speed(this._feedrate, Math.PI, "feed");
Speed sp_rapid = new Speed(this._rapidrate, Math.PI, "rapid");
// rotate workplane to account for tool twist
Transform rot = Transform.Rotation(this._tooltwist, _workplane.ZAxis, _workplane.Origin);
_workplane.Transform(rot);
// init toolpath
Toolpath prog = new Toolpath();
prog.External2 = this._extern;
prog.External1 = this._extern;
prog.Configuration = (Target.RobotConfigurations) this._config;
prog.DefFrame = _frame;
if (this._tool != null)
{
prog.DefTool = this._tool;
}
else
{
this._debug += "Tool conversion failed again...\n";
}
prog.DefSpeed = sp_rapid;
prog.DefMotion = Target.Motions.Linear;
if (this._is_drive_surface)
{
prog.InputSurface = this._drive_surface;
}
prog.Orientation = Toolpath.ToolOrientation.SurfaceNormal;
prog.Workplane = this._workplane;
for (int i = 0; i < this._toolpaths.Count; ++i)
{
if (this._toolpaths[i] == null)
{
continue;
}
// lead in
Polyline ramp = (Polyline)Util.CreateRamp(this._toolpaths[i], this._workplane, this._ramp_height, this._ramp_height * 5);
Point3d p = Util.ProjectToPlane(ramp[0], prog.Workplane);
prog.CreateTarget(p + prog.Workplane.ZAxis * (this._safe_z + this._rapid_z));
prog.CreateTarget(p + prog.Workplane.ZAxis * (this._safe_z));
// machining speed
prog.DefSpeed = sp_feed;
prog.DefMotion = Target.Motions.Linear;
for (int j = 0; j < ramp.Count; ++j)
{
prog.CreateTarget(ramp[j], Toolpath.ToolOrientation.ZAxis);
}
/*
* Point3d p = tasUtility.ProjectToPlane(this._toolpaths[i][0], prog.Workplane);
* Vector3d d2p = new Vector3d(p - this._toolpaths[i][0]);
* Vector3d ramp = new Vector3d(tasUtility.ProjectToPlane(this._toolpaths[i][1], prog.Workplane) - p);
* if (ramp.Length < this._ramp_height)
* ramp = new Vector3d(tasUtility.ProjectToPlane(this._toolpaths[i][this._toolpaths[i].Count - 2], prog.Workplane) - p);
* ramp.Unitize();
* p += ramp * this._ramp_height;
* p += prog.Workplane.ZAxis * (this._safe_z + this._rapid_z);
* prog.CreateTarget(p, Toolpath.ToolOrientation.ZAxis);
* p -= prog.Workplane.ZAxis * this._rapid_z;
* p -= d2p;
* prog.CreateTarget(p, Toolpath.ToolOrientation.ZAxis);
*/
// make toolpath targets
for (int j = 0; j < this._toolpaths[i].Count; ++j)
{
prog.CreateTarget(this._toolpaths[i][j]);
}
// lead out
Point3d p2 = Util.ProjectToPlane(this._toolpaths[i][this._toolpaths[i].Count - 1], prog.Workplane);
p2 += prog.Workplane.ZAxis * this._safe_z;
prog.DefSpeed = sp_rapid;
prog.CreateTarget(p2, Toolpath.ToolOrientation.ZAxis);
p2 += prog.Workplane.ZAxis * this._rapid_z;
//prog.DefSpeed = spr;
prog.CreateTarget(p2, Toolpath.ToolOrientation.ZAxis);
}
// yield toolpath
this._targets = prog.Targets;
this._path = prog.Path();
this._planes = prog.Planes;
DA.SetDataList("Targets", this._targets);
DA.SetData("Path", this._path);
//DA.SetDataList("Planes", this._planes);
DA.SetData("debug", this._debug);
}
