private void button1_Click(object sender, EventArgs e)
{
// X : Axis1, -5 ~ +5
// Y : Axis0, -50 ~ -60
Ojw.CTimer CTmr = new Ojw.CTimer();
CTmr.Set();
float fRang = 10;
// Move
Move(0, 10, 0, 100);
Move(10, 10, 0, 100);
Move(10, 0, 0, 100);
Move(0, 0, 0, 100);
float fXc = 5.0f;
float fYc = 5.0f;
float fZc = 0.0f;
float fLength = 2.5f;
float fRatio = 50.0f;
int nTime = 10;// 10;
for (int j = 0; j < 3; j++)
{
for (int i = 0; i < fRatio; i++)
{
Move(
fXc + fLength * Ojw.CMath.Cos(360.0f / fRatio * (i + 1)),
fYc + fLength * Ojw.CMath.Sin(360.0f / fRatio * (i + 1)),
fZc, nTime
);
}
}
//Move(0, 0, 0, 10, 10, 0, 1.0f, false);
//Move(10, 0, 0, 10, 10, 0, 1.0f, false);
//Move(10, 10, 0, 10, 10, 0, 1.0f, false);
//Move(10, 10, 0, 10, 0, 0, 1.0f, false);
//Move(10, 10, 0, 0, 0, 0, 1.0f, false);
//Move(10, 10, 0, 0, 0, 0, 1.0f, false);
Move(0, 0, 0, 1000);
// Wait
m_CMotor.Wait(m_nTime);
//Request
m_CMotor.Request_MotorData(m_nAxis);
m_CMotor.Request_Version(m_nAxis);
// Display
Ojw.CMessage.Write(
m_CMotor.GetVersion(m_nAxis).ToString() + "," +
m_CMotor.GetAngle(m_nAxis).ToString() + "," +
m_CMotor.GetTemp(m_nAxis).ToString()
);
}
private void MoveLin(float fX0, float fY0, float fZ0, float fX1, float fY1, float fZ1, float fDelta, float fSpeed)
{
m_bMoving = true;
if (fDelta <= 0)
{
Ojw.CMessage.Write_Error("fDelta = {0}", fDelta);
return;
}
if (fSpeed <= 0)
{
Ojw.CMessage.Write_Error("fSpeed = {0}", fSpeed);
return;
}
int i = 0;
m_afPos[i++] = fX1;
m_afPos[i++] = fY1;
m_afPos[i++] = fZ1;
i = 0;
m_afPos_Prev[i++] = fX0;
m_afPos_Prev[i++] = fY0;
m_afPos_Prev[i++] = fZ0;
float[] afD = new float[3];
i = 0;
for (i = 0; i < 3; i++) afD[i] = (m_afPos[i] - m_afPos_Prev[i]);
float fDistance = (float)Math.Sqrt(afD[0] * afD[0] + afD[1] * afD[1] + afD[2] * afD[2]);
int nCnt = (int)Math.Round(fDistance / fDelta, 0);
i = 0;
//float fDx = (float)Math.Abs(afD[i++]);
//float fDy = (float)Math.Abs(afD[i++]);
//float fDz = (float)Math.Abs(afD[i++]);
float fMax = 103.0f;// 103.0f; //104.0f; //22000.0f;
float fTime = fMax * fDistance * 1000.0f / fSpeed;
Ojw.CMessage.Write("Time = {0}", fTime);
Ojw.CTimer CTmr = new Ojw.CTimer();
CTmr.Set();
float fX, fY, fZ;
float fPos = 0;
for (i = 0; i < nCnt; i++)
{
fPos = (float)(i + 1) / (float)nCnt;
fX = fX0 + afD[0] * fPos;
fY = fY0 + afD[1] * fPos;
fZ = fZ0 + afD[2] * fPos;
if (i == nCnt - 1)
{
fX = m_afPos[0];
fY = m_afPos[1];
fZ = m_afPos[2];
}
//MoveRaw(m_afPos[0], m_afPos[1], m_afPos[2], (int)Math.Round(fSpeed * fPos), false);
MoveRaw(fX, fY, fZ, (int)fSpeed, false);
//Ojw.CTimer.Wait((int)fTime);
if (nCnt <= 0) nCnt = 1;
float fTimeValue = fTime * (float)(i + 1) / (float)nCnt;
if (fTimeValue <= 0) fTimeValue = 10;
while ((m_bProgEnd == false) && (m_bStop == false))
{
if (CTmr.Get() >= fTimeValue)
{
break;
}
Application.DoEvents();
}
}
//Ojw.CMessage.Write("Move End");
m_bMoving = false;
}
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 void Thread_Run()
{
Laser(false);
m_CMotor.DrvSrv(true, true);
float fRang = 0.5f;
int nTime = 40;
long lTmr = 0;
Ojw.CTimer CTmr = new Ojw.CTimer();
CTmr.Set();
// Move
Jump(-fRang, -fRang, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
for (int i = 0; i < 20; i++)
{
Draw(-fRang, fRang, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
Draw(fRang, fRang, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
Draw(fRang, -fRang, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
Draw(-fRang, -fRang, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
}
float fXc = 0.0f;
float fYc = 0.0f;
//float fZc = 0.0f;
float fLength = 2.5f;
// 만들기
int nCircle = 50;
Ojw.SVector3D_t[] aCVec = new Ojw.SVector3D_t[nCircle];
for (int i = 0; i < nCircle; i++)
{
aCVec[i].x = fXc + fLength * (float)Ojw.CMath.Cos(360.0f / (float)nCircle * (i + 1));
aCVec[i].y = fYc + fLength * (float)Ojw.CMath.Sin(360.0f / (float)nCircle * (i + 1));
}
nTime = 500;
Jump(0, 0, nTime);
lTmr += nTime; while (m_bProgEnd == false)
{
if (CTmr.Get() > lTmr)
{
break;
}
Thread.Sleep(1);
}
nTime = 20;
Jump(aCVec[nCircle - 1].x, aCVec[nCircle - 1].y, nTime);
//for (int j = 0; j < 10; j++)
//{
// for (int i = 0; i < nCircle; i++)
// {
// Draw(aCVec[i].x, aCVec[i].y, nTime);
// lTmr += nTime; while (m_bProgEnd == false) { if (CTmr.Get() > lTmr) break; Thread.Sleep(1); }
// }
//}
float fRatio = (float)nCircle;// 30.0f;
Laser(true);
nTime = 10;// 10;
Ojw.C2D m_C2d = new Ojw.C2D();
float fX, fY, fZ;
int nX, nY;
m_C2d.Create(pic2D);
m_C2d.SetAngleX(0);
m_C2d.SetAngleY(0);
m_C2d.SetAngleZ(0);
m_C2d.SetScale(10);
float fScale = 20.0f;
int nCnt = 20;
for (int j = 0; j < nCnt; j++)
{
//m_C2d.SetScale((double)fScale * Ojw.CMath.Cos((360 / nCnt) * j));
//m_C2d.SetAngleY((360 / nCnt) * j);
for (int i = 0; i < fRatio; i++)
{
fX = aCVec[i].x;
fY = aCVec[i].y;
fZ = 0;
m_C2d.Rotation(((360 / nCnt) * j), ((360 / nCnt) * j), ((360 / nCnt) * j), fX, fY, fZ, out nX, out nY);
fX = (float)nX / 10.0f;
fY = (float)nY / 10.0f;
if (i == 0)
{
Laser(false);
}
else
{
Laser(true);
}
if (i == 0)
{
Thread.Sleep(1000);
}
//else Thread.Sleep(20); //Ojw.CTimer.Wait(1);//lTmr += nTime; while (m_bProgEnd == false) { if (CTmr.Get() > lTmr) break; Thread.Sleep(1); }
Move(
fX, //aCVec[i].x,//fXc + fLength * (float)Ojw.CMath.Cos(360.0f / fRatio * ((float)i + 1.0f)),
fY, //aCVec[i].y,//fYc + fLength * (float)Ojw.CMath.Sin(360.0f / fRatio * ((float)i + 1.0f)),
m_fStep, false
);
Thread.Sleep(10);
}
}
Laser(false);
Jump(0, 0, 50);
List <Ojw.SVector3D_t> lstBox = new List <Ojw.SVector3D_t>();
lstBox.Add(new Ojw.SVector3D_t(-5, -5, -5));
lstBox.Add(new Ojw.SVector3D_t(-5, -5, 5));
lstBox.Add(new Ojw.SVector3D_t(5, -5, 5));
lstBox.Add(new Ojw.SVector3D_t(5, -5, -5));
lstBox.Add(new Ojw.SVector3D_t(-5, -5, -5));
lstBox.Add(new Ojw.SVector3D_t(-5, 5, -5));
lstBox.Add(new Ojw.SVector3D_t(-5, 5, 5));
lstBox.Add(new Ojw.SVector3D_t(5, 5, 5));
lstBox.Add(new Ojw.SVector3D_t(5, 5, -5));
lstBox.Add(new Ojw.SVector3D_t(-5, 5, -5));
float fX2, fY2, fZ2;
int nX2, nY2;
for (int j = 0; j < nCnt; j++)
{
for (int i = 1; i < lstBox.Count; i++)
{
fX = lstBox[i - 1].x;
fY = lstBox[i - 1].y;
fZ = lstBox[i - 1].z;
fX2 = lstBox[i].x;
fY2 = lstBox[i].y;
fZ2 = lstBox[i].z;
m_C2d.Rotation(((360 / nCnt) * j), ((360 / nCnt) * j), ((360 / nCnt) * j), fX, fY, fZ, out nX, out nY);
m_C2d.Rotation(((360 / nCnt) * j), ((360 / nCnt) * j), ((360 / nCnt) * j), fX2, fY2, fZ2, out nX2, out nY2);
fX = (float)nX / 100.0f;
fY = (float)nY / 100.0f;
fX2 = (float)nX2 / 100.0f;
fY2 = (float)nY2 / 100.0f;
if (i == 0)
{
Laser(false);
}
else
{
Laser(true);
}
if (i == 0)
{
Thread.Sleep(1000);
}
//else Thread.Sleep(20); //Ojw.CTimer.Wait(1);//lTmr += nTime; while (m_bProgEnd == false) { if (CTmr.Get() > lTmr) break; Thread.Sleep(1); }
Move(fX, fY, fX2, fY2, 0.02f, true);
//Move(
//fX,//aCVec[i].x,//fXc + fLength * (float)Ojw.CMath.Cos(360.0f / fRatio * ((float)i + 1.0f)),
//fY,//aCVec[i].y,//fYc + fLength * (float)Ojw.CMath.Sin(360.0f / fRatio * ((float)i + 1.0f)),
//m_fStep, false
//);
Thread.Sleep(100);
}
}
Laser(false);
// Display
Ojw.CMessage.Write("Done");
}
