private void myoHub_MyoConnected(object sender, MyoEventArgs e)
{
m_CTId0.Set();
Ojw.CMessage.Write("Myo [{0}, {1}, {2}] has connected!", e.Myo.Handle, e.Myo.XDirectionOnArm, e.Myo.Arm);
e.Myo.Vibrate(VibrationType.Short); // 접속에 성공했으니 짧게 진동 출력
e.Myo.Locked += Myo_Locked;
e.Myo.Unlocked += Myo_Unlocked;
//e.Myo.Locked += new EventHandler<MyoEventArgs>(Myo_Locked);
//e.Myo.Unlocked += new EventHandler<MyoEventArgs>(Myo_Unlocked);
#region Pose(Edge - 자세가 변하는 순간에만 기록)
e.Myo.PoseChanged += Myo_PoseChanged;
#endregion Pose(Edge - 자세가 변하는 순간에만 기록)
#region Pose(자세가 계속적으로 기록...)
// setup for the pose we want to watch for
m_myoPos = HeldPose.Create(e.Myo, Pose.Fist, Pose.FingersSpread, Pose.WaveIn, Pose.WaveOut, Pose.Rest);
// set the interval for the event to be fired as long as
// the pose is held by the user
m_myoPos.Interval = TimeSpan.FromSeconds(0.5);
m_myoPos.Start();
m_myoPos.Triggered += Pose_Triggered;
#endregion Pose(자세가 계속적으로 기록...)
e.Myo.Unlock(UnlockType.Hold); // 이걸 마지막에 선언하면 Myo 가 내버려 두어도 Lock 이 되지 않는다.
#region Orientation
e.Myo.OrientationDataAcquired += Myo_OrientationDataAcquired;
#endregion Orientation
}
private void myoHub_MyoConnected(object sender, MyoEventArgs e)
{
Ojw.CMessage.Write(String.Format("Myo {0} has connected!", e.Myo.Handle));
e.Myo.Vibrate(VibrationType.Short);
//e.Myo.Unlock(UnlockType.Hold);
Ojw.CMessage.Write("Connected(Myo)");
m_CTId.Set();
e.Myo.EmgDataAcquired += Myo_EmgDataAcquired;
e.Myo.SetEmgStreaming(true);
}
private void Myo_OrientationDataAcquired(object sender, OrientationDataEventArgs e)
{
const float PI = (float)System.Math.PI;
int nDev = 1; //10;
// convert the values to a 0-9 scale (for easier digestion/understanding)
float nRoll = (float)((e.Roll + PI) / (PI * 2.0f) * nDev);
float nPitch = (float)((e.Pitch + PI) / (PI * 2.0f) * nDev);
float nYaw = (float)((e.Yaw + PI) / (PI * 2.0f) * nDev);
if (m_CTId0.Get() >= 1000)
{
m_CTId0.Set();
//if (m_bFirst == true)
//{
// m_afInitAngle[0] = e.Orientation.X;
// m_afInitAngle[1] = e.Orientation.Y;
// m_afInitAngle[2] = e.Orientation.W;
//}
float fX = (float)Math.Round(Ojw.CMath.R2D(e.Orientation.X - m_afInitAngle[0]), 3);
float fY = (float)Math.Round(Ojw.CMath.R2D(e.Orientation.Y - m_afInitAngle[1]), 3);
float fW = (float)Math.Round(Ojw.CMath.R2D(e.Orientation.W - m_afInitAngle[2]), 3);
//float fSwing = (float)Math.Round(Ojw.CMath.R2D(e.Roll), 3);
//float fTilt = (float)Math.Round(Ojw.CMath.R2D(e.Pitch), 3);
//float fPan = (float)Math.Round(Ojw.CMath.R2D(e.Yaw), 3);
//m_C3d.SetRobot_Rot(fPan - 90.0f, -fTilt, -fSwing);
Ojw.CMessage.Write("[{6}]Roll[{0}], Pitch[{1}], Yaw[{2}] || X[{3}], Y[{4}], W[{5}]", nRoll, nPitch, nYaw, fX, fY, fW, e.Myo.Handle);
}
}
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 Myo_EmgDataAcquired(object sender, EmgDataEventArgs e)
{
// Display Emg Text Data (1000 ms interval = 1 second)
if (m_CTId.Get() >= 1000)
{
m_CTId.Set();
Ojw.CMessage.Write(String.Format("Emg = {0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}",
e.EmgData.GetDataForSensor(0),
e.EmgData.GetDataForSensor(1),
e.EmgData.GetDataForSensor(2),
e.EmgData.GetDataForSensor(3),
e.EmgData.GetDataForSensor(4),
e.EmgData.GetDataForSensor(5),
e.EmgData.GetDataForSensor(6),
e.EmgData.GetDataForSensor(7)));
}
// Display Emg Graphic Data (100 ms interval)
if (m_CTId_Graph.Get() >= 100)
{
m_CTId_Graph.Set();
m_CGrap.Push(
e.EmgData.GetDataForSensor(0),
e.EmgData.GetDataForSensor(1),
e.EmgData.GetDataForSensor(2),
e.EmgData.GetDataForSensor(3),
e.EmgData.GetDataForSensor(4),
e.EmgData.GetDataForSensor(5),
e.EmgData.GetDataForSensor(6),
e.EmgData.GetDataForSensor(7)
);
m_CGrap.OjwDraw();
}
}
private void FJoystick_Check_Alive()
{
#region Joystick Check
Color m_colorLive = Color.LightGreen; // 살았을 경우의 색
Color m_colorDead = Color.Gray; // 죽었을 경우의 색
if (m_CJoy.IsValid == false)
{
#region 조이스틱이 연결되지 않았음을 표시
if (lbJoystick.ForeColor != m_colorDead)
{
lbJoystick.Text = "Joystick (No Connected)";
lbJoystick.ForeColor = m_colorDead;
}
#endregion 조이스틱이 연결되지 않았음을 표시
#region 3초마다 다시 재연결을 하려고 시도
if (m_CTmr_Joystick.Get() > 3000) // Joystic 이 죽어있다면 체크(3초단위)
{
Ojw.CMessage.Write("Joystick Check again");
m_CJoy = new Ojw.CJoystick(Ojw.CJoystick._ID_0);
if (m_CJoy.IsValid == false)
{
Ojw.CMessage.Write("But we can't find a joystick device in here. Check your joystick device");
m_CTmr_Joystick.Set(); // 타이머의 카운터를 다시 초기화 한다.
}
else
{
Ojw.CMessage.Write("Joystick is Connected");
}
}
#endregion 3초마다 다시 재연결을 하려고 시도
}
else
{
#region 연결 되었음을 표시
if (lbJoystick.ForeColor != m_colorLive)
{
lbJoystick.Text = "Joystick (Connected)";
lbJoystick.ForeColor = m_colorLive;
}
#endregion 연결 되었음을 표시
}
#endregion Joystick Check
}
private Ojw.CTimer HMC_Timer = new Ojw.CTimer(); // Timer to check the joystick connection periodically
private void Joystick_Check_Connection()
{
#region Joystick Check
Color Connected_Color = Color.LightGreen; // Color when connected
Color Disconnected_Color = Color.Yellow; // Color when disconnected
if (HMController.IsValid == false)
{
#region Joystick is not connected
if (lbJoystick.ForeColor != Disconnected_Color)
{
lbJoystick.Text = "Joystick (No Connected)";
lbJoystick.ForeColor = Disconnected_Color;
}
#endregion Joystick is not connected
#region Try to reconnect every 3 seconds
if (HMC_Timer.Get() > 3000) // Check if Joystic is disconnected (every 3 seconds)
{
Ojw.CMessage.Write("Joystick Check again");
HMController = new Ojw.CJoystick(Ojw.CJoystick._ID_0);
if (HMController.IsValid == false)
{
Ojw.CMessage.Write("We can't find a joystick device in your PC. Check your Joystick");
HMC_Timer.Set(); // Reset the timer counter again.
}
else
{
Ojw.CMessage.Write("Joystick is Connected");
}
}
#endregion Try to reconnect every 3 seconds
}
else
{
#region Connected
if (lbJoystick.ForeColor != Connected_Color)
{
lbJoystick.Text = "Joystick (Connected)";
lbJoystick.ForeColor = Connected_Color;
}
#endregion Connected
}
#endregion Joystick Check
}
//joystick bağlantısını kontrol ediyoruz
private void JoystickCheckAlive()
{
Color colorAlive = Color.Green;
Color colorDeath = Color.Gray;
if (m_CJoy.IsValid == false)
{
if (lbJoystick.ForeColor != colorDeath)
{
lbJoystick.Text = "Joystick (No Connected)";
lbJoystick.ForeColor = colorDeath;
buttonTimer.Enabled = false;
mouseTimer.Enabled = false;
stopOrStart.Enabled = false;
}
if (m_CTmr_Joystick.Get() > 3000)
{
Ojw.CMessage.Write("Joystick Check again");
m_CJoy = new Ojw.CJoystick(Ojw.CJoystick._ID_0);
if (m_CJoy.IsValid == false)
{
Ojw.CMessage.Write("But we can't find a joystick device in here. Check your joystick device");
m_CTmr_Joystick.Set();
}
else
{
Ojw.CMessage.Write("Joystick is Connected");
}
}
}
else
{
if (lbJoystick.ForeColor != colorAlive)
{
lbJoystick.Text = "Joystick (Connected)";
lbJoystick.ForeColor = colorAlive;
}
mouseTimer.Enabled = true;
buttonTimer.Enabled = true;
stopOrStart.Enabled = true;
}
}
private void FJoystick_Check_Data()
{
#region JoyStick
// [ 아래는 XBox 기준이다. ]
// 스틱좌상단, 0+ - 좌, 1+ - 하
// 패드 2+ - 좌, 3+ - 하
// 스틱우하단, 4+ - 좌, 5+ - 하
// 버튼
// Top 6 - -3 : 클릭
// Left 7 - -3 : 클릭
// Right 8 - -3 : 클릭
// Bottom 9 - -3 : 클릭
// 전면우측 10 - -3 : 클릭
// 전면좌측 11 - -3 : 클릭
// 전면우측아래 12- : 클릭
// 전면좌측아래 13- : 클릭
if (m_CJoy.IsValid == true)
{
//// 좌상단 조이스틱
if (Math.Abs(m_CJoy.dX0 - 0.5) > 0.2)
{
if (m_CTmr_Throat.Get() > 100)
{
double dData = (double)Math.Abs(m_CJoy.dX0 - 0.5);
if (dData > 0)
{
//1448 +- 300
double dValue = 1448 + (dData * 2) * 300;// (dData * 2) * 100.0;
int nValue2 = (int)((dData * 2) * 65535);
nValue2 = (nValue2 != 0) ? 1 : 0;
Ojw.CMessage.Write("Data = {0}", dValue);
SendValue(1, nValue2, 1, nValue2, 1, (int)dValue, 1, (int)dValue, 1, (int)dValue);
}
else
{
//SendValue(0, 0, 0, 0, 1, 0, 1, 0, 1, 0);
}
m_CTmr_Throat.Set();
}
m_C3d.SetData(1, (float)((m_CJoy.dX0 - 0.5) / 5.0 * 10.0));
m_C3d.SetData(2, (float)((m_CJoy.dX0 - 0.5) / 5.0 * 10.0));
}
else
{
if (m_CTmr_Throat.Get() > 100)
{
m_CTmr_Throat.Set();
}
m_C3d.SetData(1, 0);
m_C3d.SetData(2, 0);
}
if (Math.Abs(m_CJoy.dY1 - 0.5) > 0.2)
{
m_C3d.SetData(0, (float)10.0f); //((m_CJoy.dY0 - 0.5) / 5.0 * 10.0));
}
else
{
m_C3d.SetData(0, 0);
}
#if _XBOX //
int nValue = 65535;
if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button5) == true)
//if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button7) == true)
{
//m_CSocket.Send(Ojw.CConvert.StrToBytes("0"));
m_C3d.SetColor_Second(0, 4, 4, 0, Color.Cyan);
//nRight = 100;
//Ojw.CMessage.Write("Clicked 9");
Ojw.CMessage.Write("Light Right 100");
SendValue(1, nValue, 1, nValue, 0, 0, 0, 0, 0, 0);
}
else if (m_CJoy.IsUp_Event(Ojw.CJoystick.PadKey.Button5) == true)
{
//nRight = 0;
m_C3d.SetColor_Second(0, 4, 4, 0, Color.Black);
Ojw.CMessage.Write("Light Right off");
SendValue(1, 0, 1, 0, 0, 0, 0, 0, 0, 0);
}
if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button6) == true)
//if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button8) == true)
{
//nLeft = 100;
m_C3d.SetColor_Second(1, 5, 5, 0, Color.Cyan);
//m_CSocket.Send(Ojw.CConvert.StrToBytes("1"));
Ojw.CMessage.Write("Light Left 100");
//Ojw.CMessage.Write("Clicked 10");
SendValue(1, nValue, 1, nValue, 0, 0, 0, 0, 0, 0);
}
else if (m_CJoy.IsUp_Event(Ojw.CJoystick.PadKey.Button6) == true)
{
//nLeft = 0;
m_C3d.SetColor_Second(1, 5, 5, 0, Color.Black);
Ojw.CMessage.Write("Light Left off");
SendValue(1, 0, 1, 0, 0, 0, 0, 0, 0, 0);
}
#else
if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button7) == true)
{
m_C3d.SetColor_Second(0, 4, 4, 0, Color.Cyan);
Ojw.CMessage.Write("Clicked 9");
SendValue(1, 100, 0, 0, 0, 0, 0, 0);
}
else if (m_CJoy.IsUp_Event(Ojw.CJoystick.PadKey.Button7) == true)
{
m_C3d.SetColor_Second(0, 4, 4, 0, Color.Black);
SendValue(1, 0, 0, 0, 0, 0, 0, 0);
}
if (m_CJoy.IsDown_Event(Ojw.CJoystick.PadKey.Button8) == true)
{
m_C3d.SetColor_Second(1, 5, 5, 0, Color.Cyan);
Ojw.CMessage.Write("Clicked 10");
SendValue(0, 0, 1, 100, 0, 0, 0, 0);
}
else if (m_CJoy.IsUp_Event(Ojw.CJoystick.PadKey.Button8) == true)
{
m_C3d.SetColor_Second(1, 5, 5, 0, Color.Black);
SendValue(0, 0, 1, 0, 0, 0, 0, 0);
}
#endif
// 이 부분이 어떤 버튼이 눌리는지를 화면상에 뿌려준다.
for (int i = 0; i < 30; i++)
{
if (m_CJoy.IsDown_Event((Ojw.CJoystick.PadKey)i) == true)
{
Ojw.CMessage.Write("Button Down{0}", i);
}
if (m_CJoy.IsUp_Event((Ojw.CJoystick.PadKey)i) == true)
{
Ojw.CMessage.Write("Button Up{0}", i);
}
}
}
#endregion JoyStick
}
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 btnSet_Click(object sender, EventArgs e)
{
m_CTId_0.Set();
}
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");
}
