void ParseAxis(NcCodeCmd cmd)
{
int lineIndex = -1;
for (; ;)
{
if (_Token == null)
{
break;
}
// 行番号が変わったら他のコマンドとなる
if (lineIndex < 0)
{
lineIndex = _Token.LineIndex;
}
else
{
if (lineIndex != _Token.LineIndex)
{
break;
}
}
bool noMatch = false;
switch (_Token.Symbol)
{
case 'X':
cmd.Flags |= NcCodeFlags.X;
cmd.X = cmd.OrgX = Convert.ToDouble(_Token.Arg);
break;
case 'Y':
cmd.Flags |= NcCodeFlags.Y;
cmd.Y = cmd.OrgY = Convert.ToDouble(_Token.Arg);
break;
case 'Z':
cmd.Flags |= NcCodeFlags.Z;
cmd.Z = cmd.OrgZ = Convert.ToDouble(_Token.Arg);
break;
default:
noMatch = true;
break;
}
if (noMatch)
{
break;
}
NextToken();
}
if ((cmd.Flags & (NcCodeFlags.X | NcCodeFlags.Y | NcCodeFlags.Z)) == 0)
{
throw new ApplicationException("軸が指定されていません。");
}
}
void ParseP(NcCodeCmd cmd)
{
if (_Token != null && _Token.Symbol == 'P')
{
cmd.Flags |= NcCodeFlags.P;
cmd.P = Convert.ToInt32(_Token.Arg);
NextToken();
}
else
{
throw new ApplicationException("サブプログラム番号の指定がありません。");
}
}
void ParseR(NcCodeCmd cmd)
{
if (_Token != null && _Token.Symbol == 'R')
{
cmd.Flags |= NcCodeFlags.R;
cmd.R = Convert.ToDouble(_Token.Arg);
NextToken();
}
else
{
throw new ApplicationException("半径の指定がありません。");
}
}
/// <summary>
/// 現在の絶対／相対モードと指定コマンドによりポジションを更新する、コマンドのXYZは機械座標系に書き換わる
/// </summary>
/// <param name="state">状態</param>
/// <param name="cmd">コマンド</param>
static void MovePosition(State state, NcCodeCmd cmd)
{
if (state.AbsoluteMode)
{
// 絶対モードならコマンドの座標をそのまま現在座標に設定
// G53モード中なら機械座標が指定されたものとするためオフセットは０になる
var offset = state.G53Processing ? new Vector3d(0.0, 0.0, 0.0) : state.WorkO;
if ((cmd.Flags & NcCodeFlags.X) != 0)
{
state.MachinePosition.X = cmd.X + offset.X;
cmd.X = state.MachinePosition.X;
}
if ((cmd.Flags & NcCodeFlags.Y) != 0)
{
state.MachinePosition.Y = cmd.Y + offset.Y;
cmd.Y = state.MachinePosition.Y;
}
if ((cmd.Flags & NcCodeFlags.Z) != 0)
{
state.MachinePosition.Z = cmd.Z + offset.Z;
cmd.Z = state.MachinePosition.Z;
}
}
else
{
// 相対モードなら現在座標に加算し、コマンド側を加算後の絶対座標で書き換える
if ((cmd.Flags & NcCodeFlags.X) != 0)
{
state.MachinePosition.X += cmd.X;
cmd.X = state.MachinePosition.X;
}
if ((cmd.Flags & NcCodeFlags.Y) != 0)
{
state.MachinePosition.Y += cmd.Y;
cmd.Y = state.MachinePosition.Y;
}
if ((cmd.Flags & NcCodeFlags.Z) != 0)
{
state.MachinePosition.Z += cmd.Z;
cmd.Z = state.MachinePosition.Z;
}
}
// 移動したら解除
state.G53Processing = false;
}
void ParseL(NcCodeCmd cmd, bool isOptional)
{
if (_Token != null && _Token.Symbol == 'L')
{
cmd.Flags |= NcCodeFlags.L;
cmd.L = Convert.ToInt32(_Token.Arg);
NextToken();
}
else
{
if (!isOptional)
{
throw new ApplicationException("ループ回数の指定がありません。");
}
}
}
void ParseF(NcCodeCmd cmd, bool isOptional)
{
if (_Token != null && _Token.Symbol == 'F')
{
cmd.Flags |= NcCodeFlags.F;
cmd.F = Convert.ToDouble(_Token.Arg);
NextToken();
}
else
{
if (!isOptional)
{
throw new ApplicationException("速度の指定がありません。");
}
}
}
/// <summary>
/// 自分と指定コマンドの実行内容が同じかどうか調べる
/// </summary>
/// <param name="cmd">判定対象コマンド</param>
/// <returns>true:同じ、false:異なる</returns>
public bool CmdEquals(NcCodeCmd cmd)
{
if (this.CmdType != cmd.CmdType)
{
return(false);
}
if (this.Flags != cmd.Flags)
{
return(false);
}
if (this.X != cmd.X)
{
return(false);
}
if (this.Y != cmd.Y)
{
return(false);
}
if (this.Z != cmd.Z)
{
return(false);
}
if (this.R != cmd.R)
{
return(false);
}
if (this.F != cmd.F)
{
return(false);
}
if (this.S != cmd.S)
{
return(false);
}
if (this.P != cmd.P)
{
return(false);
}
if (this.L != cmd.L)
{
return(false);
}
return(true);
}
/// <summary>
/// 構造解析しコマンド配列を作成する
/// </summary>
public List <NcCodeCmd> Parse()
{
try {
var list = new List <NcCodeCmd>();
int code;
bool skippable = false;
while (_Token != null)
{
NcCodeCmd cmd = null;
var oldCount = list.Count;
switch (_Token.Symbol)
{
case 'G':
// Gコード解析
code = Convert.ToInt32(_Token.Arg);
switch (code)
{
case 0:
// 位置決め
_LastCmdType = NcCodeCmdType.G00;
break;
case 1:
// 直線補間
_LastCmdType = NcCodeCmdType.G01;
break;
case 2:
// 円弧補間(CW)
_LastCmdType = NcCodeCmdType.G02;
break;
case 3:
// 円弧補間(CCW)
_LastCmdType = NcCodeCmdType.G03;
break;
case 4:
// ドウェル ※ウェイト
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G04;
cmd.Flags = NcCodeFlags.XYTime;
cmd.XYTime = Convert.ToInt32(_Token.Data) / 1000.0;
list.Add(cmd);
break;
case 17:
// XY平面指定
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G17;
list.Add(cmd);
break;
case 18:
// ZX平面指定
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G18;
list.Add(cmd);
break;
case 19:
// YZ平面指定
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G19;
list.Add(cmd);
break;
case 53:
// 機械座標系選択
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G53;
list.Add(cmd);
break;
case 90:
// アブソリュート指令
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G90;
list.Add(cmd);
break;
case 91:
// インクリメンタル指令
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.G91;
list.Add(cmd);
break;
case 92:
// 座標系設定
_LastCmdType = NcCodeCmdType.G92;
break;
default:
throw new ApplicationException("未対応のG" + code + "が出現しました。");
}
NextToken();
break;
case 'M':
// Mコード解析
code = Convert.ToInt32(_Token.Arg);
cmd = new NcCodeCmd(_Token.LineIndex);
switch (code)
{
case 0:
// プログラムストップ
cmd.CmdType = NcCodeCmdType.M00;
NextToken();
break;
case 1:
// オプショナルストップ
cmd.CmdType = NcCodeCmdType.M01;
NextToken();
break;
case 2:
// プログラムエンド
cmd.CmdType = NcCodeCmdType.M02;
NextToken();
break;
case 10:
// IO処理？
cmd.CmdType = NcCodeCmdType.M10;
NextToken();
break;
case 20:
// IO処理？
cmd.CmdType = NcCodeCmdType.M20;
NextToken();
break;
case 98:
// サブプログラム呼び出し
cmd.CmdType = NcCodeCmdType.M98;
NextToken();
ParseP(cmd);
ParseL(cmd, true);
break;
case 99:
// サブプログラム終了
cmd.CmdType = NcCodeCmdType.M99;
NextToken();
break;
default:
throw new ApplicationException("未対応のM" + code + "が出現しました。");
}
list.Add(cmd);
break;
case 'O':
// サブプログラム番号
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.O;
cmd.Flags |= NcCodeFlags.P;
cmd.P = Convert.ToInt32(_Token.Arg);
list.Add(cmd);
NextToken();
break;
case 'X':
case 'Y':
case 'Z':
// 軸指定
if (_LastCmdType == NcCodeCmdType.None)
{
throw new ApplicationException("Gコマンドでのモーダル開始前に軸の指定が出現しました。");
}
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = _LastCmdType;
ParseAxis(cmd);
if (_LastCmdType == NcCodeCmdType.G02 || _LastCmdType == NcCodeCmdType.G03)
{
ParseR(cmd);
}
ParseF(cmd, true);
list.Add(cmd);
break;
case 'S':
// スピンドル
cmd = new NcCodeCmd(_Token.LineIndex);
cmd.CmdType = NcCodeCmdType.S;
cmd.S = Convert.ToDouble(_Token.Arg);
list.Add(cmd);
NextToken();
break;
case ';':
// コメントアウト
NextToken();
break;
case '/':
// スキップ可能
NextToken();
skippable = true;
break;
default:
throw new ApplicationException("未対応のコマンド '" + _Token.Symbol + "' が出現しました。");
}
// スキップ可能状態の最中に新しいコマンドが来たらそれにスキップ可能フラグをセットする
if (skippable && oldCount != list.Count)
{
skippable = false;
list[oldCount].Flags |= NcCodeFlags.IsSkippable;
}
}
return(list);
} catch (Exception ex) {
throw new NcCodeParserException(ex.Message, this);
}
}
public NcCodeParserException(string message, NcCodeCmd cmd) :
base(cmd.PositionDesc + message)
{
this.LineIndex = cmd.LineIndex;
this.Position = 0;
}
/// <summary>
/// コマンドから動きをシミュレートし、移動所要時間、移動距離、円弧補間情報などセットする
/// </summary>
/// <param name="state">状態</param>
/// <param name="cmd">コマンド</param>
public static void Simulate(State state, NcCodeCmd cmd)
{
switch (cmd.CmdType)
{
case NcCodeCmdType.G00:
// 位置決め
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
var move = cmd.EndPos - cmd.StartPos;
var time = move.Abs() / state.MachineSpeed;
cmd.XYTime = 60.0 * time.Max; // 全要素の中の最大値を取得
cmd.XYLength = move.Length; // 移動距離
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
break;
}
case NcCodeCmdType.G01:
// 直線補間
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
if ((cmd.Flags & NcCodeFlags.F) != 0)
{
state.Speed = cmd.F;
}
var move = cmd.EndPos - cmd.StartPos;
var time = 60.0 * new Vector2d(move.X, move.Y).Length / state.Speed;
cmd.XYTime = time; // 移動時間 ※コントローラが２軸の補間しかできないらしいので平面の距離から時間計算している
cmd.XYLength = move.Length; // 移動距離
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
break;
}
case NcCodeCmdType.G02:
case NcCodeCmdType.G03:
// 円弧補間
{
cmd.StartPos = state.MachinePosition;
MovePosition(state, cmd);
cmd.EndPos = state.MachinePosition;
if ((cmd.Flags & NcCodeFlags.F) != 0)
{
state.Speed = cmd.F;
}
if (cmd.StartPos == cmd.EndPos)
{
// 始点と終点が完全に同じ、無視して良い命令
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength;
cmd.XYLength = 0.0; // 移動距離
cmd.XYTime = 0.0; // 移動時間
break;
}
var start2 = TransformPlane(state.Plane, cmd.StartPos);
var end2 = TransformPlane(state.Plane, cmd.EndPos);
if (start2 == end2)
{
var planeName = state.Plane.ToString();
throw new WarningException((cmd.LineIndex + 1) + "行目: 円弧補間の" + planeName + "始点と終点が同じで他軸値だけ異なっています。");
}
var arc = new ArcInterpolation(start2, end2, cmd.R, cmd.CmdType == NcCodeCmdType.G02);
if (arc.NoArcCenter)
{
throw new WarningException((cmd.LineIndex + 1) + "行目: 円弧計算解なし");
}
cmd.XYLength = Math.Abs(arc.Distance * arc.Gamma); // 移動距離
cmd.XYTime = 60.0 * cmd.XYLength / state.Speed; // 移動時間
cmd.Plane = state.Plane;
cmd.Arc = arc;
cmd.Flags |= NcCodeFlags.StartPos | NcCodeFlags.EndPos | NcCodeFlags.XYTime | NcCodeFlags.XYLength | NcCodeFlags.Arc;
break;
}
case NcCodeCmdType.G17:
// XY平面指定
state.Plane = PlaneEnum.XY;
break;
case NcCodeCmdType.G18:
// ZX平面指定
state.Plane = PlaneEnum.ZX;
break;
case NcCodeCmdType.G19:
// YZ平面指定
state.Plane = PlaneEnum.YZ;
break;
case NcCodeCmdType.G53:
// 機械座標系選択
// ※NCMATEにあわせるためワンショット扱い、次の移動命令が来たら解除される
state.G53Processing = true;
break;
case NcCodeCmdType.G90:
// アブソリュート指令
state.AbsoluteMode = true;
break;
case NcCodeCmdType.G91:
// インクリメンタル指令
state.AbsoluteMode = false;
break;
case NcCodeCmdType.G92:
// ワーク座標設定
state.WorkO = state.MachinePosition - new Vector3d(cmd.OrgX, cmd.OrgY, cmd.OrgZ);
break;
}
}
