protected void ReadGroup1(CNCInstruction baseInstr) { if (isStartingTask) { CNCInstructionMotion instr = (CNCInstructionMotion)baseInstr; scaleVector = Vector3.one * instr.prefixModifier; target = pieceOriginTransform.TransformPoint(Vector3.Scale(instr.TargetPos, scaleVector)); if (gCode == 2 || gCode == 3) { InitializeG02_03(instr); // Spiral Checking float currentRadius = (TargetKnife.transform.position - pivot).magnitude; float targetRadius = (target - pivot).magnitude; if (!CheckRadiusMatched(currentRadius, targetRadius) && AllowSpiral) { PrintError("Warning: Radius Error, motion might be spiral.\n"); } } } switch (gCode) { case 0: LinearTraverseTo(target, maxDistanceRapid); break; case 1: LinearTraverseTo(target, maxDistanceFeed); break; case 2: CircularTraverseTo(target, pivot, maxDistanceFeed, angleDiff, radiusDiff); if (IsAtTargetCrude(target)) { isFinishedTask = true; } break; case 3: CircularTraverseTo(target, pivot, maxDistanceFeed, angleDiff, radiusDiff); if (IsAtTargetCrude(target)) { isFinishedTask = true; } break; default: PrintError("Unknown G Code falling into ReadGroup1\n"); break; } if (IsAtTarget(target)) { isFinishedTask = true; } }
private void SendCNCInstruction(int modalGroup) { CNCInstruction instr; switch (modalGroup) { case 1: instr = new CNCInstructionMotion { G = gCodeForEachGroup[1], Group = modalGroup, prefixModifier = this.prefixModifier, FeedRate = this.feedRate, SpindleSpeed = this.spindleSpeed, Tool = this.tool, MiscFunc = this.miscFunc, posX = this.posX, posY = this.posY, posZ = this.posZ, posI = this.posI, posJ = this.posJ, posK = this.posK }; break; default: instr = new CNCInstruction { G = this.gCode, Group = modalGroup, prefixModifier = this.prefixModifier, FeedRate = this.feedRate, SpindleSpeed = this.spindleSpeed, Tool = this.tool, MiscFunc = this.miscFunc }; print("Invalid modal group Found"); if (DebugVR != null) { DebugVR.Println("Invalid modal group found"); } break; } TargetInstrList.AddLast(instr); }
public virtual void GetBackHome() { if (InstructionList.Count == 0) { CNCInstructionMotion tempInstr1 = new CNCInstructionMotion { G = 0, Group = 1, prefixModifier = 0.001f, FeedRate = 10f, SpindleSpeed = 0, Tool = 1, MiscFunc = 0 }; Vector3 tempPos1 = PieceOrigin.transform.InverseTransformPoint(new Vector3(TargetKnife.transform.position.x, Home.transform.position.y, TargetKnife.transform.position.z)) * 1000f; tempInstr1.TargetPos = new Vector3(tempPos1.x, tempPos1.y, tempPos1.z); tempInstr1.PivotRelPos = Vector3.zero; CNCInstructionMotion tempInstr2 = new CNCInstructionMotion { G = 0, Group = 1, prefixModifier = 0.001f, FeedRate = 10f, SpindleSpeed = 0, Tool = 1, MiscFunc = 0 }; Vector3 tempPos2 = PieceOrigin.transform.InverseTransformPoint(new Vector3(Home.transform.position.x, Home.transform.position.y, Home.transform.position.z)) * 1000f; tempInstr2.TargetPos = new Vector3(tempPos2.x, tempPos2.y, tempPos2.z); tempInstr2.PivotRelPos = Vector3.zero; InstructionList.AddLast(tempInstr1); InstructionList.AddLast(tempInstr2); PrintlnWithVR("2 G00 Instructions added to get back home."); } }
protected virtual void InitializeG02_03(CNCInstructionMotion instr) { /*scaleVector = Vector3.one * instr.prefixModifier; * target = pieceOriginTransform.TransformPoint(Vector3.Scale(instr.TargetPos, scaleVector));*/ pivot = pieceOriginTransform.TransformDirection(Vector3.Scale(instr.PivotRelPos, scaleVector)) + TargetKnife.transform.position; axis = AxisForRotation(TargetKnife.transform.position, target, pivot); // Determining the direction of the axis Vector3 initialDir = TargetKnife.transform.position - pivot; Vector3 targetDir = target - pivot; radiusDiff = targetDir.magnitude - initialDir.magnitude; float initialAngle = Arctan0360(initialDir.z, initialDir.x); float targetAngle = Arctan0360(targetDir.z, targetDir.x); angleDiff = Clamp0360(-(targetAngle - initialAngle)); // Ensuring the axis to always be Clockwise rotated as seen from top view if (axis.y < 0) { axis *= -1; } // Hardcode for 180degrees if (Mathf.Abs(angleDiff - 180f) < 0.1f || axis == Vector3.zero) { axis = Quaternion.FromToRotation(new Vector3(initialDir.x, 0, initialDir.z), initialDir) * Vector3.up; } // Reverse the axis for Counter-Clockwise rotation (G03) if (gCode == 3) { axis *= -1; angleDiff = Clamp0360(-angleDiff); } }
// Checks all instruction in InstructionListIn and changes their container into InstructionList // If any CNCInstruction is invalid in any form, this will break putting them into into InstructionList // This method's coordinates are in CNC coordination protected void CheckInstructionListIn(float startX, float startY, float startZ) { bool isValid = true; int g; float x_last, y_last, z_last; float x_now, y_now, z_now; x_last = startX; y_last = startY; z_last = startZ; foreach (CNCInstruction instr in InstructionListIn) { if (instr.FeedRate * instr.prefixModifier > LimitFeedRate) { print(instr.FeedRate + " * " + instr.prefixModifier + " > " + LimitFeedRate); isValid = false; PrintlnWithVR("Feed Rate exceeds limit\n" + instr.ToStringShort()); } g = instr.G; switch (instr.Group) { case 1: CNCInstructionMotion instrG1 = (CNCInstructionMotion)instr; x_now = instrG1.posX; y_now = instrG1.posY; z_now = instrG1.posZ; if (!isValidBoundary(x_now, y_now, z_now)) { isValid = false; PrintlnWithVR("Coordinate exceeds limit\n" + instrG1.ToStringShort()); } if ((g == 2 || g == 3) && !AllowSpiral) { // Absolute I,J,K float i_ab = x_last + instrG1.posI; float j_ab = y_last + instrG1.posJ; float k_ab = z_last + instrG1.posK; float radius1 = Vector3.Distance(new Vector3(x_last, y_last, z_last), new Vector3(i_ab, j_ab, k_ab)); float radius2 = Vector3.Distance(new Vector3(x_now, y_now, z_now), new Vector3(i_ab, j_ab, k_ab)); if (!CheckRadiusMatched(radius1, radius2)) { isValid = false; PrintlnWithVR("Radius error\n" + instrG1.ToStringShort()); } } x_last = x_now; y_last = y_now; z_last = z_now; break; } if (!isValid) { break; } else { InstructionList.AddLast(instr); } } if (!isValid) { ClearInstrQueue(); } else { InstructionListIn.Clear(); } }