private bool LoadFile_GCode(String strFileName, out SMotion_t SMotion)
{
bool bRet = true;
// Init
SMotion.nCnt_Frame = 0;
SMotion.pSFrame = null;
FileInfo f = null;
FileStream fs = null;
float fSpeedBack = 0;
try
{
f = new FileInfo(strFileName);
fs = f.OpenRead();
byte byteData;
byte[] pbyteData = new byte[4096];
//fs.Read(pbyteData, 0, pbyteData.Length);
//fs.Close();
///////////////////////////////////////
SMotion.nCnt_Frame = 0;
SMotion.pSFrame = new SMotionFrame_t[1];
double dRatio = 1.0;// 0.2;
//String strLine = "";
//int nStartIndex = 0;
int nLength = 0;
pbyteData.Initialize();
//double dX0 = 0, dY0 = m_dWaitPos_Y, dZ0 = 0;
double dStep = 0;
//m_bNeedWaitTime = true;
//OjwMotor.SetLed_Red(3, false);
//Dispense_Insert();
//Move_Init();
// 이때부터 디스펜싱이 시작된다.
//m_bStart = true;
Ojw.CTimer.Wait(10000);
//COjwTimer.TimerSet(0);
Ojw.CTimer CTmr = new Ojw.CTimer();
CTmr.Set();
double dEvalue = 0.0f;
double dFvalue = 0.0f;
bool bFirstMove = true;
float fDelta = Ojw.CConvert.StrToFloat(txtDelta.Text);
float fSpeed;
float fMulti = Ojw.CConvert.StrToFloat(txtMulti.Text);
//int nPos = 0;
for (int i = 0; i < fs.Length; i++) // 너무 기니까 테스트로 1/10만...
{
lbProgress.Text = Ojw.CConvert.IntToStr(i + 1) + "/" + Ojw.CConvert.IntToStr(fs.Length + 1) + "[" + ((float)(i + 1) / (float)fs.Length * 100.0f).ToString() + "%]"; ;
if (m_bStop == true) break;
byteData = (byte)(fs.ReadByte() & 0xff);
pbyteData[nLength++] = byteData;
if (byteData == 0x0a)
{
//strLine = BitConverter.ToString(pbyteData, i, nLength);
SMotionFrame_t SFrame = new SMotionFrame_t();
//if (InterPreter_Gcode_to_Frame(Encoding.Default.GetString(pbyteData, nStartIndex, nLength), out SFrame) == true)
if (InterPreter_Gcode_to_Frame(Encoding.Default.GetString(pbyteData, 0, nLength), out SFrame) == true)
{
#if false
Array.Resize<SMotionFrame_t>(ref SMotion.pSFrame, SMotion.nCnt_Frame + 1);
//SMotion.pSFrame[i] = SFrame;
SMotion.pSFrame[SMotion.nCnt_Frame].bEn = SFrame.bEn;
SMotion.pSFrame[SMotion.nCnt_Frame].bDispense = SFrame.bDispense;
SMotion.pSFrame[SMotion.nCnt_Frame].bX = SFrame.bX;
SMotion.pSFrame[SMotion.nCnt_Frame].dX = SFrame.dX * dRatio;// +m_fOffset_X;
SMotion.pSFrame[SMotion.nCnt_Frame].bY = SFrame.bY;
SMotion.pSFrame[SMotion.nCnt_Frame].dY = SFrame.dY;// *dRatio; // 높이는 제외
SMotion.pSFrame[SMotion.nCnt_Frame].bZ = SFrame.bZ;
SMotion.pSFrame[SMotion.nCnt_Frame].dZ = SFrame.dZ * dRatio;// +m_fOffset_Y;
SMotion.pSFrame[SMotion.nCnt_Frame].bEValue = SFrame.bEValue;
SMotion.pSFrame[SMotion.nCnt_Frame].dEValue = SFrame.dEValue;
SMotion.pSFrame[SMotion.nCnt_Frame].bFValue = SFrame.bFValue;
SMotion.pSFrame[SMotion.nCnt_Frame].dFValue = SFrame.dFValue;
double dX, dY, dZ;
dX = ((SMotion.pSFrame[SMotion.nCnt_Frame].bX) ? SFrame.dX : dX0);// +m_fOffset_X;
dY = (SMotion.pSFrame[SMotion.nCnt_Frame].bY) ? m_dWorkPos_Y + SFrame.dY : dY0;
dZ = ((SMotion.pSFrame[SMotion.nCnt_Frame].bZ) ? SFrame.dZ : dZ0);// +m_fOffset_Y;
//dStep = m_dStep;// = ((SFrame.bEValue == true) && (SFrame.dEValue > 0)) ? m_dStep : 1.0;
double dMulti = 1.0;
dMulti = m_dParam_Multi;//1.0;// 1.0f;// 1.0;// 2.0;
//if (SFrame.dFValue > 0)
// dStep = SFrame.dFValue / 10000.0 * dMulti; // 3배속
//else if (SFrame.dFValue == 0) dStep = m_dStep;
// else 기존값 유지
if (SFrame.bFValue == true)
{
dFvalue = SFrame.dFValue;
}
//const int _SPD2 = 5;
const int _SPD3 = 1;
dStep = m_dStep;
if (dFvalue > 0)
{
m_nSpeed = (int)Math.Round((100000.0 / dMulti) / (dFvalue), 0);
//if (m_nSpeed < _SPD2) m_nSpeed = _SPD2;
if (m_nSpeed < (int)m_dParam_Limit) m_nSpeed = (int)m_dParam_Limit;
}
else m_nSpeed = _SPD3; //
// dStep = dFvalue / 10000.0 * dMulti; // 3배속
String str0 = "E[" + Ojw.CConvert.DoubleToStr(SFrame.dEValue) + "]";
//String str1 = "dStep[" + Ojw.CConvert.DoubleToStr(dStep) + "]" + "F[" + Ojw.CConvert.DoubleToStr(SFrame.dFValue) + "]";
String str1 = "Spd[" + Ojw.CConvert.IntToStr(m_nSpeed) + "]" + "F[" + Ojw.CConvert.DoubleToStr(SFrame.dFValue) + "]";
lbDisp.Text = str1 + ((SFrame.bEValue == true) ? str0 : "");
bool bLimit = false;
double dLimit = 1;// 0.5;// 1.0;
//if (dStep > dLimit)
if (dFvalue / 10000.0 > dLimit)
{
//dStep = dLimit;
bLimit = true;
m_nSpeed = (int)m_dParam_Jump_Speed;
}
else if (dStep <= 0) dStep = 0.0001;
if (
(bLimit == true) &&
(
(dX0 != dX) ||
(dY0 != dY) ||
(dZ0 != dZ)
)
)
{
double dAlpha = m_dParam_Jump;
Move(dX0, dY0, dZ0,
dX0,
dY0 + dAlpha,
dZ0,
dStep, !bFirstMove); //k++;
Move(dX0, dY0 + dAlpha, dZ0, dX, dY + dAlpha, dZ, dStep, true);
Move(dX, dY + dAlpha, dZ, dX, dY, dZ, dStep, true);
}
else
{
Move(dX0, dY0, dZ0, dX, dY, dZ, dStep, !bFirstMove);
}
bFirstMove = false;
dX0 = dX;
dY0 = dY;
dZ0 = dZ;
#else
//SFrame.bEn
float fX = (SFrame.bX == true) ? (float)SFrame.dX : m_afPos_Prev[0];
float fY = (SFrame.bY == true) ? (float)SFrame.dY : m_afPos_Prev[1];
float fZ = (SFrame.bZ == true) ? (float)SFrame.dZ : m_afPos_Prev[2];
bool bMove = (
(SFrame.bX == true) ||
(SFrame.bY == true) ||
(SFrame.bZ == true)
) ? true : false;
if (bMove == true)
{
fSpeed = (SFrame.bFValue == true) ? (float)SFrame.dFValue : fSpeedBack;
fSpeedBack = fSpeed;
//fSpeed = (SMotion.nCnt_Frame == 0) ? 7000.0f : (float)SFrame.dFValue / 100.0f;
//if (fSpeed <= 0) fSpeed = 100.0f;
MoveLin(
m_afPos_Prev[0], m_afPos_Prev[1], m_afPos_Prev[2],
fX, fY, fZ, fDelta, fSpeed * fMulti);
//Send(Ojw.CConvert.RemoveCaption(Encoding.Default.GetString(pbyteData, 0, nLength), false, false));
//Ojw.CTimer.Wait(10);
#endif
}
else
{
Send(Ojw.CConvert.RemoveCaption(Encoding.Default.GetString(pbyteData, 0, nLength), false, false));
}
// Dispense
if (SFrame.bDispense == true)
{
}
SMotion.nCnt_Frame++;
}
Application.DoEvents();
pbyteData.Initialize();
nLength = 0;
}
}
if (fs != null) fs.Close();
if (m_bStop == true)
{
bRet = false;
}
//Dispense_Stop();
//OjwMotor.SetLed_Red(3, true);
return bRet;
}
catch
{
if (fs != null) fs.Close();
//Dispense_Stop();
return false;
}
}
private bool InterPreter_Gcode_to_Frame(String strLine, out SMotionFrame_t SMotion)
{
SMotion.nGCode = -1;
SMotion.bEn = false;
SMotion.bDispense = false;
SMotion.bEValue = false;
SMotion.dEValue = 0.0;
SMotion.bFValue = false;
SMotion.dFValue = 0.0;
SMotion.bX = SMotion.bY = SMotion.bZ = false;
SMotion.dX = SMotion.dY = SMotion.dZ = 0.0;
// 주석 제거
int nEnd = strLine.IndexOf(';');
if (nEnd >= 0)
{
if (nEnd == 0) return false;
strLine = strLine.Substring(0, nEnd + 1);
}
if (strLine == null) return false;
if (strLine.Length < 1) return false;
//strLine = Ojw.CConvert.ChangeString(strLine, " ", ":");
//strLine = Ojw.CConvert.RemoveString(strLine, " ");
String[] pstrValue = strLine.Split(' ');
foreach (String strItem in pstrValue)
{
String strData = Ojw.CConvert.RemoveString(strItem, " ").ToUpper();
if (strData.Length > 0)
{
if (strData[0] == 'G')
{
SMotion.nGCode = Ojw.CConvert.StrToInt(strData.Substring(1, strData.Length - 1));
SMotion.bEn = true;
//switch (SMotion.nGCode)
//{
// case 1:
// {
// SMotion.bEn = true;
// }
// break;
//}
}
if (SMotion.bEn == true)
{
if (strData[0] == 'E')
{
SMotion.bEValue = true;
SMotion.dEValue = Ojw.CConvert.StrToDouble(strData.Substring(1, strData.Length - 1));
//if (SMotion.dEValue < 0) // Wipe 인 경우는 현재 I/O 가 없으므로 구현 안한다.
//{
// SMotion.bEn = false;
//}
}
else if (strData[0] == 'F')
{
SMotion.bFValue = true;
SMotion.dFValue = Ojw.CConvert.StrToDouble(strData.Substring(1, strData.Length - 1));
}
//else if (strData[0] == 'X')
else if (strData[0] == 'Z')
{
SMotion.bZ = true;
SMotion.dZ = Ojw.CConvert.StrToDouble(strData.Substring(1, strData.Length - 1));
}
//else if (strData[0] == 'Y')
else if (strData[0] == 'X')
{
SMotion.bX = true;
SMotion.dX = Ojw.CConvert.StrToDouble(strData.Substring(1, strData.Length - 1));
}
//else if (strData[0] == 'Z')
else if (strData[0] == 'Y')
{
SMotion.bY = true;
SMotion.dY = Ojw.CConvert.StrToDouble(strData.Substring(1, strData.Length - 1));
}
}
}
}
return SMotion.bEn;
}
