private CNCState.CNCState ProcessSyncToolCommand(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
var cmd = block.Options.FirstOrDefault((arg) => (arg.letter == 'M'));
if (cmd == null)
{
return(state);
}
int tool;
if (block.SingleOptions.ContainsKey('T'))
{
tool = block.SingleOptions['T'].ivalue1;
}
else
{
tool = state.SyncToolState.Tool;
}
switch (cmd.ivalue1)
{
case 703:
return(builder.ProcessSyncToolStart(tool, program, state));
case 705:
return(builder.ProcessSyncToolStop(tool, program, state));
}
return(state);
}
private CNCState.CNCState ProcessToolCommand(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
var cmd = block.Options.FirstOrDefault((arg) => (arg.letter == 'M'));
if (cmd == null)
{
return(state);
}
Dictionary <string, decimal> options = new Dictionary <string, decimal>();
// Rotation direction
int command = cmd.ivalue1;
if (command == 3)
{
options["ccw"] = 0;
}
else if (command == 4)
{
options["ccw"] = 1;
}
// Tool ID
int tool_id;
if (cmd.dot)
{
tool_id = cmd.ivalue2;
}
else
{
tool_id = config.deftool_id;
}
// Rotation speed
var S = block.Options.FirstOrDefault((arg) => arg.letter == 'S');
if (S != null)
{
options["speed"] = S.value;
}
switch (cmd.ivalue1)
{
case 3:
case 4:
return(builder.ProcessToolStart(tool_id, options, program, state));
case 5:
return(builder.ProcessToolStop(tool_id, program, state));
}
return(state);
}
private CNCState.CNCState ProcessCoordinatesSet(Arguments args,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
decimal?X = GetValue(args.X, state);
decimal?Y = GetValue(args.Y, state);
decimal?Z = GetValue(args.Z, state);
return(builder.CoordinatesSet(X, Y, Z, program, state));
}
private CNCState.CNCState ProcessParameters(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
state = state.BuildCopy();
if (block.Feed != null)
{
state.AxisState.Feed = ProgramBuilder.ConvertSizes(GetValue(block.Feed, state).Value, state) / 60.0m; // convert from mm/min to mm/sec
state.AxisState.Feed = Math.Max(state.AxisState.Feed, config.basefeed);
}
return(state);
}
private CNCState.CNCState ProcessMove(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
if (!state.DrillingState.Drilling)
{
return(ProcessDirectMove(block, program, state));
}
else
{
return(ProcessDrillingMove(block, program, state));
}
}
private CNCState.CNCState ProcessDrillingMove(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
// TODO Handle G17, 18, 19
decimal?X = GetValue(block.X, state);
decimal?Y = GetValue(block.Y, state);
decimal?Z = GetValue(block.Z, state); // Drill depth
decimal?R = GetValue(block.R, state); // Retract
decimal?Q = GetValue(block.Q, state); // Pecking
return(builder.ProcessDrillingMove(X, Y, Z, R, Q, program, state));
}
private CNCState.CNCState ProcessCoordinatesSystemSet(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
var cmd = block.Options.FirstOrDefault((arg) => (arg.letter == 'G'));
if (cmd == null)
{
return(state);
}
if (cmd.ivalue1 < 53 || cmd.ivalue1 > 59)
{
return(state);
}
return(builder.CoordinatesSystemSet(cmd.ivalue1 - 53, program, state));
}
private (CNCState.CNCState, bool) Process(Arguments frame, ActionProgram.ActionProgram program, CNCState.CNCState state)
{
if (frame.SingleOptions.ContainsKey('X') && frame.SingleOptions.ContainsKey('Y'))
{
return(ProcessDrillingMove(frame, program, state), false);
}
else if (frame.SingleOptions.ContainsKey('T'))
{
if (!frame.SingleOptions.ContainsKey('C'))
{
var tool = frame.SingleOptions['T'].ivalue1;
state = builder.ProcessToolChange(tool, program, state);
program.AddBreak(state);
return(state, true);
}
}
return(state, false);
}
public (ActionProgram.ActionProgram actionProgram, ProgramBuildingState finalState, IReadOnlyDictionary <IAction, (int procedure, int line)> actionLines, string errorMessage) BuildProgram(CNCState.CNCState initialMachineState, ProgramBuildingState builderState)
{
var program = new ActionProgram.ActionProgram(rtSender, modbusSender, config, machine, toolManager);
var actionLines = new Dictionary <IAction, (int, int)>();
var state = builder.BeginProgram(program, initialMachineState);
actionLines[program.Actions[0].action] = (-1, -1);
Sequence sequence = builderState.Source.Procedures[builderState.CurrentProcedure];
state = builder.ProcessDrillingMove(null, null, -1, 10, 10, program, state);
bool finish = false;
builderState.Completed = false;
while (!finish)
{
bool stop;
if (builderState.CurrentLine >= sequence.Lines.Count)
{
break;
}
Logger.Instance.Debug(this, "build", string.Format("processing line {0}", builderState.CurrentLine));
Arguments frame = sequence.Lines[builderState.CurrentLine];
try
{
var len0 = program.Actions.Count;
(state, stop) = Process(frame, program, state);
var len1 = program.Actions.Count;
if (len1 > len0)
{
Logger.Instance.Debug(this, "build", "has actions");
var first = program.Actions[len0].action;
actionLines[first] = (builderState.CurrentProcedure, builderState.CurrentLine);
}
else
{
Logger.Instance.Debug(this, "build", "Doesn't have actions");
}
}
catch (Exception e)
{
var msg = String.Format("{0} : {1}", frame, e.ToString());
Logger.Instance.Error(this, "compile error", msg);
return(null, null, new Dictionary <IAction, (int, int)>(), e.Message);
}
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;
builderState.CurrentLine += 1;
if (stop)
{
break;
}
}
if (builderState.CurrentLine >= sequence.Lines.Count)
{
builderState.Completed = true;
}
state = builder.FinishProgram(program, state);
return(program, builderState, actionLines, "");
}
private CNCState.CNCState ProcessDirectMove(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
decimal?X = GetValue(block.X, state);
decimal?Y = GetValue(block.Y, state);
decimal?Z = GetValue(block.Z, state);
decimal?I = GetValue(block.I, state);
decimal?J = GetValue(block.J, state);
decimal?K = GetValue(block.K, state);
decimal?R = GetValue(block.R, state);
CNCState.AxisState.MType moveType;
var cmd = block.Options.FirstOrDefault((arg) => arg.letter == 'G');
if (cmd != null)
{
switch (cmd.ivalue1)
{
case 0:
moveType = AxisState.MType.FastLine;
break;
case 1:
moveType = AxisState.MType.Line;
break;
case 2:
moveType = AxisState.MType.ArcCW;
break;
case 3:
moveType = AxisState.MType.ArcCCW;
break;
default:
return(state);
}
}
else
{
moveType = state.AxisState.MoveType;
}
state.AxisState.MoveType = moveType;
switch (moveType)
{
case AxisState.MType.FastLine:
return(builder.ProcessLineMove(X, Y, Z, true, program, state));
case AxisState.MType.Line:
return(builder.ProcessLineMove(X, Y, Z, false, program, state));
case AxisState.MType.ArcCW:
return(builder.ProcessArcMove(X, Y, Z, I, J, K, R, false, program, state));
case AxisState.MType.ArcCCW:
return(builder.ProcessArcMove(X, Y, Z, I, J, K, R, true, program, state));
}
return(state);
}
private (CNCState.CNCState, ProgramBuilderCommand command, int programid, int amount) Process(Arguments args,
ActionProgram.ActionProgram program,
CNCState.CNCState state,
int curprogram,
int curline,
Dictionary <IAction, (int, int)> starts)
ProcessBlock(Arguments block,
ActionProgram.ActionProgram program,
CNCState.CNCState state)
{
var cmd = block.Options.FirstOrDefault((arg) => (arg.codeType == Arguments.Option.CodeType.Code && (arg.letter == 'G' || arg.letter == 'M')));
ProgramBuilderCommand command = ProgramBuilderCommand.Continue;
int pid = -1, amount = -1;
state = state.BuildCopy();
state = ProcessParameters(block, program, state);
foreach (var opt in block.Options)
{
if (opt.codeType == Arguments.Option.CodeType.Variable)
{
var val = GetValue(opt, state);
string varid = opt.varid;
state = state.BuildCopy();
state.VarsState.Vars[varid] = val.Value;
}
}
if (cmd == null)
{
state = ProcessMove(block, program, state);
}
else if (cmd.letter == 'G')
{
switch (cmd.ivalue1)
{
case 0:
case 1:
case 2:
case 3:
state = ProcessMove(block, program, state);
break;
case 4:
try
{
if (block.SingleOptions.ContainsKey('P'))
{
var P = block.SingleOptions['P'];
var dt = GetValue(P, state).Value;
state = builder.AddPause(dt, program, state);
}
}
catch
{
;
}
break;
case 17:
state.AxisState.Params.CurrentPlane = AxisState.Plane.XY;
break;
case 18:
state.AxisState.Params.CurrentPlane = AxisState.Plane.YZ;
break;
case 19:
state.AxisState.Params.CurrentPlane = AxisState.Plane.ZX;
break;
case 20:
state.AxisState.Params.SizeUnits = AxisState.Units.Inches;
break;
case 21:
state.AxisState.Params.SizeUnits = AxisState.Units.Millimeters;
break;
case 28:
state = builder.AddHoming(program, state);
break;
case 30:
state = builder.AddZProbe(program, state);
break;
case 53:
case 54:
case 55:
case 56:
case 57:
case 58:
case 59:
state = ProcessCoordinatesSystemSet(block, program, state);
break;
case 80:
state.DrillingState.Drilling = false;
state = ProcessMove(block, program, state);
break;
case 81:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Rapid;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 82:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Rapid;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = true;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 83:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = true;
state.DrillingState.Retract = DrillingState.RetractType.Rapid;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 84:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Feed;
state.DrillingState.RetractReverse = true;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = true;
state = ProcessMove(block, program, state);
break;
case 85:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Feed;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 86:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Feed;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = true;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 87:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Manual;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = false;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = true;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 88:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Manual;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = true;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = true;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 89:
state.DrillingState.Drilling = true;
state.DrillingState.Peck = false;
state.DrillingState.Retract = DrillingState.RetractType.Feed;
state.DrillingState.RetractReverse = false;
state.DrillingState.Dwell = true;
state.DrillingState.LeftHand = false;
state.DrillingState.StopSpindle = false;
state.DrillingState.Tapping = false;
state = ProcessMove(block, program, state);
break;
case 90:
state.AxisState.Absolute = true;
break;
case 91:
state.AxisState.Absolute = false;
break;
case 92:
state = ProcessCoordinatesSet(block, program, state);
break;
case 98:
state.DrillingState.RetractDepth = DrillingState.RetractDepthType.InitialHeight;
break;
case 99:
state.DrillingState.RetractDepth = DrillingState.RetractDepthType.RHeight;
break;
}
}
else if (cmd.letter == 'M')
{
switch (cmd.ivalue1)
{
case 0:
program.AddBreak(state);
command = ProgramBuilderCommand.Pause;
break;
case 2:
program.AddStop(state);
command = ProgramBuilderCommand.Finish;
break;
case 3:
case 4:
case 5:
state = ProcessToolCommand(block, program, state);
break;
case 6:
if (block.SingleOptions.ContainsKey('T'))
{
// TODO: stop spindle
int tool = block.SingleOptions['T'].ivalue1;
state = builder.ProcessToolChange(tool, program, state);
if (toolManager.ToolChangeInterrupts)
{
command = ProgramBuilderCommand.Pause;
}
}
break;
case 97:
command = ProgramBuilderCommand.Call;
(pid, amount) = CallSubprogram(block, state);
break;
case 99:
command = ProgramBuilderCommand.Return;
break;
case 120:
builder.PushState(state.AxisState);
break;
case 121:
builder.PopState(state.AxisState);
break;
case 703:
case 705:
state = ProcessSyncToolCommand(block, program, state);
break;
}
}
return(state, command, pid, amount);
}
