public void ProcessProgram(ActionProgram.ActionProgram program)
{
ExecutionProgram = program;
if (program == null)
{
ExecutionTime = 0;
return;
}
decimal time = 0;
foreach (var action in program.Actions)
{
if (!(action.action is RTAction ma))
{
continue;
}
if (!(ma.Command is IRTMoveCommand move))
{
continue;
}
var len = move.Length;
var feed = move.Options.Feed;
var t = len / feed;
time += t;
}
ExecutionTime = time;
}
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 ProcessToolStart(int tool_id,
IReadOnlyDictionary <string, decimal> options,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
IToolState toolState = state.ToolStates[tool_id];
if (toolState is SpindleState ss)
{
ss.SpindleSpeed = options["speed"];
if (options["ccw"] != 0)
{
ss.RotationState = SpindleState.SpindleRotationState.CounterClockwise;
}
else
{
ss.RotationState = SpindleState.SpindleRotationState.Clockwise;
}
}
else if (toolState is BinaryState bs)
{
bs.Enabled = true;
}
return(CommitTool(tool_id, program, state));
}
public CNCState.CNCState ProcessToolChange(int tool, ActionProgram.ActionProgram program, CNCState.CNCState state)
{
var newstate = state.BuildCopy();
program.AddToolChange(tool); // change tool
return(newstate);
}
public CNCState.CNCState CoordinatesSystemSet(int cs,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
state.AxisState.Params.CurrentCoordinateSystemIndex = cs;
return(state);
}
public CNCState.CNCState AddZProbe(ActionProgram.ActionProgram program, CNCState.CNCState state)
{
var after = state.BuildCopy();
after.AxisState.Position.z = 0;
program.AddZProbe(state, after);
program.AddAction(new SyncCoordinates(stateSyncManager, state.AxisState.Position), state, null);
return(state);
}
public CNCState.CNCState ProcessSyncToolStop(int tool_id,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
var newstate = state.BuildCopy();
newstate.SyncToolState.Tool = tool_id;
newstate.SyncToolState.Enabled = false;
program.DisableRTTool(tool_id, state, newstate);
return(newstate);
}
public void ProcessProgram(ActionProgram.ActionProgram program)
{
List <MoveActionChain> chains = new List <MoveActionChain>();
MoveActionChain chain = new MoveActionChain();
var actions = program.Actions;
Logger.Instance.Debug(this, "optimize", "begin optimization");
int index = 1;
int na = actions.Count;
foreach (var action in actions)
{
Logger.Instance.Debug(this, "optimize", string.Format("Process action #{0} / {1}", index, na));
var ma = action.action as RTAction;
if (ma == null || ma.RequireFinish || ma.Command as IRTMoveCommand == null)
{
if (ma == null || ma.RequireFinish)
{
if (chain.Actions.Count > 0)
{
var subc = SplitChainByDirectionFlip(chain);
chains.AddRange(subc);
chain = new MoveActionChain();
}
}
}
else
{
var cmd = ma.Command as IRTMoveCommand;
chain.Actions.Add(cmd);
}
index += 1;
}
if (chain.Actions.Count > 0)
{
var subc = SplitChainByDirectionFlip(chain);
chains.AddRange(subc);
}
index = 1;
int nc = chains.Count;
foreach (var c in chains)
{
Logger.Instance.Debug(this, "optimize", string.Format("Optimize chain #{0} / {1}. It has {2} actions", index, nc, c.Actions.Count));
OptimizeChain(c);
index += 1;
}
Logger.Instance.Debug(this, "optimize", "ready optimization");
}
public CNCState.CNCState FinishProgram(ActionProgram.ActionProgram program, CNCState.CNCState state)
{
program.AddPlaceholder(state);
Logger.Instance.Debug(this, "build", "finish build program");
Logger.Instance.Debug(this, "build", "move feed limit");
moveFeedLimiter.ProcessProgram(program);
Logger.Instance.Debug(this, "build", "optimize program");
optimizer.ProcessProgram(program);
Logger.Instance.Debug(this, "build", "calculate time of execution");
timeCalculator.ProcessProgram(program);
Logger.Instance.Debug(this, "build", string.Format("program ready. Time = {0}", timeCalculator.ExecutionTime));
return(state);
}
public CNCState.CNCState BeginProgram(ActionProgram.ActionProgram program, CNCState.CNCState state)
{
state = state.BuildCopy();
var currentPos = state.AxisState.Params.CurrentCoordinateSystem.ToLocal(state.AxisState.TargetPosition);
state.VarsState.Vars["x"] = currentPos.x;
state.VarsState.Vars["y"] = currentPos.y;
state.VarsState.Vars["z"] = currentPos.z;
Logger.Instance.Debug(this, "build", "start build program");
program.AddConfiguration(state);
program.AddRTUnlock(state);
return(state);
}
public CNCState.CNCState ProcessToolStop(int tool_id,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
IToolState toolState = state.ToolStates[tool_id];
if (toolState is SpindleState ss)
{
ss.RotationState = SpindleState.SpindleRotationState.Off;
}
else if (toolState is BinaryState bs)
{
bs.Enabled = false;
}
return(CommitTool(tool_id, program, state));
}
private CNCState.CNCState CommitTool(int tool_id,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
IAction action = null;
IDriver driver = tool_drivers[tool_id];
if (driver is N700E_driver n700e)
{
SpindleState ss = state.ToolStates[tool_id] as SpindleState;
action = n700e.CreateAction(ss.RotationState, ss.SpindleSpeed);
}
else if (driver is GPIO_driver gpio)
{
BinaryState bs = state.ToolStates[tool_id] as BinaryState;
action = gpio.CreateAction(bs.Enabled);
}
else if (driver is RawModbus_driver modbus)
{
BinaryState bs = state.ToolStates[tool_id] as BinaryState;
action = modbus.CreateAction(bs.Enabled);
}
else if (driver is Dummy_driver dummy)
{
action = dummy.CreateAction();
}
else
{
throw new InvalidOperationException("invalid driver: " + driver);
}
if (action != null)
{
program.AddAction(action, state, state);
return(state);
}
else
{
return(state);
}
}
public CNCState.CNCState CoordinatesSet(decimal?X, decimal?Y, decimal?Z,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
if (X != null)
{
state.AxisState.Params.CurrentCoordinateSystem.Offset.x =
state.AxisState.Position.x - ConvertSizes(X.Value, state);
}
if (Y != null)
{
state.AxisState.Params.CurrentCoordinateSystem.Offset.y =
state.AxisState.Position.y - ConvertSizes(Y.Value, state);
}
if (Z != null)
{
state.AxisState.Params.CurrentCoordinateSystem.Offset.z =
state.AxisState.Position.z - ConvertSizes(Z.Value, state);
}
state.AxisState.TargetPosition = state.AxisState.Position;
return(state);
}
public void ProcessProgram(ActionProgram.ActionProgram program)
{
foreach (var action in program.Actions)
{
if (!(action.action is RTAction ma))
{
continue;
}
if (ma.Command is RTLineMoveCommand linemovecmd)
{
Vector3 dir = linemovecmd.DirStart; // Equal to DirEnd
var(maxfeed, maxacc) = MaxLineFeedAcc(dir);
linemovecmd.Options.acceleration = maxacc;
linemovecmd.Options.Feed = Math.Min(maxfeed, linemovecmd.Options.Feed);
}
if (ma.Command is RTArcMoveCommand arcmovecmd)
{
decimal R = arcmovecmd.R;
decimal maxfeed = (decimal)Math.Sqrt((double)(R * config.max_acceleration));
arcmovecmd.Options.Feed = Math.Min(arcmovecmd.Options.Feed, maxfeed);
}
}
}
public ExpectedTimeCalculator()
{
ExecutionTime = 0;
ExecutionProgram = null;
}
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);
}
public CNCState.CNCState ProcessArcMove(decimal?X, decimal?Y, decimal?Z,
decimal?I, decimal?J, decimal?K, decimal?R,
bool ccw,
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 (R == null && I == null && J == null && K == null)
{
decimal r;
switch (state.AxisState.Axis)
{
case AxisState.Plane.XY:
r = (new Vector2(delta.x, delta.y)).Length() / 2;
break;
case AxisState.Plane.YZ:
r = (new Vector2(delta.y, delta.z)).Length() / 2;
break;
case AxisState.Plane.ZX:
r = (new Vector2(delta.z, delta.x)).Length() / 2;
break;
default:
throw new InvalidOperationException();
}
return(program.AddArcMovement(delta, compensation, r, ccw, state.AxisState.Axis, state.AxisState.Feed, state));
}
else if (R != null)
{
var r = ConvertSizes(R.Value, state);
return(program.AddArcMovement(delta, compensation, r, ccw, state.AxisState.Axis, state.AxisState.Feed, state));
}
else
{
decimal i = 0;
decimal j = 0;
decimal k = 0;
if (I != null)
{
i = ConvertSizes(I.Value, state);
}
if (J != null)
{
j = ConvertSizes(J.Value, state);
}
if (K != null)
{
k = ConvertSizes(K.Value, state);
}
return(program.AddArcMovement(delta, compensation, new Vector3(i, j, k), ccw, state.AxisState.Axis, state.AxisState.Feed, state));
}
}
public CNCState.CNCState AddPause(decimal dt, ActionProgram.ActionProgram program, CNCState.CNCState state)
{
program.AddDelay((int)dt, state);
return(state);
}
