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();
}
}