public CNCState.CNCState ProcessLineMove(decimal?X, decimal?Y, decimal?Z,
bool fast,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
if (X == null && Y == null && Z == null)
{
return(state);
}
state = state.BuildCopy();
Vector3 delta;
Vector3 compensation;
Vector3 targetPosition;
(delta, compensation, targetPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, X, Y, Z);
state.AxisState.TargetPosition = targetPosition;
if (fast)
{
return(program.AddFastLineMovement(delta, compensation, state));
}
else
{
return(program.AddLineMovement(delta, compensation, state.AxisState.Feed, state));
}
}
public CNCState.CNCState ProcessDrillingMove(decimal?X, decimal?Y, decimal?Z, decimal?R, decimal?Q,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
// TODO Handle G17, 18, 19
Vector3 delta, compensation;
var coordinateSystem = state.AxisState.Params.CurrentCoordinateSystem;
if (R != null)
{
state.DrillingState.RetractHeightLocal = R.Value;
}
if (Z != null)
{
state.DrillingState.DrillHeightLocal = Z.Value;
}
if (Q != null)
{
state.DrillingState.PeckDepth = Math.Abs(Q.Value);
}
if (state.DrillingState.Peck && state.DrillingState.PeckDepth == 0)
{
throw new InvalidOperationException("Pecking with depth = 0");
}
#region Positioning
if (X == null && Y == null)
{
return(state);
}
Vector3 topPosition;
(delta, compensation, topPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, X, Y, null);
state.AxisState.TargetPosition = topPosition;
state = program.AddFastLineMovement(delta, compensation, state);
#endregion Positioning
var absmode = state.AxisState.Absolute;
state.AxisState.Absolute = true;
#region R height
Vector3 rPosition;
(delta, compensation, rPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, state.DrillingState.RetractHeightLocal);
state.AxisState.TargetPosition = rPosition;
state = program.AddFastLineMovement(delta, compensation, state);
#endregion R height
// TODO: dwelling, etc
Vector3 currentDrillPosition = state.AxisState.TargetPosition;
Vector3 bottomPosition;
(_, _, bottomPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, state.DrillingState.DrillHeightLocal);
decimal startHeight = coordinateSystem.ToLocal(rPosition).z;
decimal preparationHeight = startHeight;
decimal currentHeight = startHeight;
decimal finishHeight = coordinateSystem.ToLocal(bottomPosition).z;
decimal peckMax;
if (state.DrillingState.Peck)
{
peckMax = state.DrillingState.PeckDepth;
}
else
{
peckMax = decimal.MaxValue;
}
while (currentHeight != finishHeight)
{
decimal targetHeight;
decimal deltaHeight = finishHeight - currentHeight;
if (deltaHeight > peckMax)
{
deltaHeight = peckMax;
}
if (deltaHeight < -peckMax)
{
deltaHeight = -peckMax;
}
targetHeight = currentHeight + deltaHeight;
#region preparation
if (preparationHeight != startHeight)
{
(delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, preparationHeight);
state.AxisState.TargetPosition = currentDrillPosition;
state = program.AddFastLineMovement(delta, compensation, state);
}
#endregion
#region drilling
(delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, targetHeight);
state.AxisState.TargetPosition = currentDrillPosition;
state = program.AddLineMovement(delta, compensation, state.AxisState.Feed, state);
#endregion
#region retracting
(delta, compensation, currentDrillPosition) = FindMovement(state, state.AxisState.TargetPosition, state.AxisState.Position, null, null, startHeight);
state.AxisState.TargetPosition = currentDrillPosition;
state = program.AddFastLineMovement(delta, compensation, state);
#endregion
preparationHeight = currentHeight;
currentHeight = targetHeight;
}
#region Retract
switch (state.DrillingState.RetractDepth)
{
case DrillingState.RetractDepthType.InitialHeight:
delta = topPosition - state.AxisState.TargetPosition;
compensation = state.AxisState.TargetPosition - state.AxisState.Position;
state.AxisState.TargetPosition = topPosition;
state = program.AddFastLineMovement(delta, compensation, state);
break;
case DrillingState.RetractDepthType.RHeight:
break;
default:
throw new InvalidOperationException("Unknown retract depth state");
}
#endregion
// Restore mode
state.AxisState.Absolute = absmode;
return(state);
}
