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 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 Myo_OrientationDataAcquired(object sender, OrientationDataEventArgs e)
{
if (e.Myo.Arm == Arm.Left)
{
m_bLeft = true;
const float PI = (float)System.Math.PI;
int nDev = 1; //10;
// convert the values to a 0-9 scale (for easier digestion/understanding)
float fRoll = (float)((e.Roll + PI) / (PI * 2.0f) * nDev);
float fPitch = (float)((e.Pitch + PI) / (PI * 2.0f) * nDev);
float fYaw = (float)((e.Yaw + PI) / (PI * 2.0f) * nDev);
if (m_CTId0.Get() >= 1000)
{
m_CTId0.Set();
Ojw.CMessage.Write("Pitch={0}", fPitch);
if (m_ER_Pose == Pose.FingersSpread)
{
if ((fPitch < 0.6f) && (fPitch >= 0.4f))
{
// 전진
Ojw.CMessage.Write("Forward");
m_nCommand = 1;
MoveForward();
//QWalk(_CNT_WALK, 0, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
else if (fPitch >= 0.6f)
{
// 후진
Ojw.CMessage.Write("Backward");
m_nCommand = 2;
MoveBackward();
//QWalk(_CNT_WALK, 3, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
}
else if (m_ER_Pose == Pose.Fist)
{
if (fPitch >= 0.6f)
{
//if (m_nCommand != 3)
{
Ojw.CMessage.Write("Stand up");
m_nCommand = 3;
//Ojw.CTimer.Wait(1000);
//ActionPlay("80");
//Stop();
MoveTurnLeft();
}
}
else if (fPitch < 0.4f)
{
if (m_nCommand != 4)
{
Ojw.CMessage.Write("have a seat");
m_nCommand = 4;
//Ojw.CTimer.Wait(1000);
//ActionPlay("40");
MoveTurnRight();// Stop();
}
}
else
{
Stop(); // QWalk_Stop(0); // Stop
}
}
else if (m_ER_Pose == Pose.WaveOut)
{
if (m_nCommand != 5)
{
Ojw.CMessage.Write("Right");
m_nCommand = 5;
MoveLeft();
//QWalk(_CNT_WALK, 1, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0)); // Right
}
}
else if (m_ER_Pose == Pose.WaveIn)
{
if (m_nCommand != 6)
{
Ojw.CMessage.Write("Left");
m_nCommand = 6;
MoveRight();// MoveLeft();
//QWalk(_CNT_WALK, 2, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
}
}
m_fR_Roll = fRoll;
m_fR_Pitch = fPitch;
m_fR_Yaw = fYaw;
}
else if (e.Myo.Arm == Arm.Right)
{
m_bRight = true;
const float PI = (float)System.Math.PI;
int nDev = 1; //10;
// convert the values to a 0-9 scale (for easier digestion/understanding)
float fRoll = (float)((e.Roll + PI) / (PI * 2.0f) * nDev);
float fPitch = (float)((e.Pitch + PI) / (PI * 2.0f) * nDev);
float fYaw = (float)((e.Yaw + PI) / (PI * 2.0f) * nDev);
m_fL_Roll = fRoll;
m_fL_Pitch = fPitch;
m_fL_Yaw = fYaw;
}
}
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 void btnGet_Click(object sender, EventArgs e)
{
long lData = m_CTId_0.Get();
lbGet.Text = lData.ToString() + " ms";
}
private void btnGet_Click(object sender, EventArgs e)
{
lbTime.Text = Ojw.CConvert.IntToStr(m_CTmr.Get()); // Same => m_CTmr.Get().ToString()
}
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");
}
private void Myo_OrientationDataAcquired(object sender, OrientationDataEventArgs e)
{
if (e.Myo.Arm == Arm.Left)
{
m_bLeft = true;
const float PI = (float)System.Math.PI;
int nDev = 1; //10;
// convert the values to a 0-9 scale (for easier digestion/understanding)
float fRoll = (float)((e.Roll + PI) / (PI * 2.0f) * nDev);
float fPitch = (float)((e.Pitch + PI) / (PI * 2.0f) * nDev);
float fYaw = (float)((e.Yaw + PI) / (PI * 2.0f) * nDev);
if (m_CTId0.Get() >= 1000)
{
m_CTId0.Set();
#if false
Ojw.CMessage.Write("Pitch={0}", fPitch);
if (m_ER_Pose == Pose.FingersSpread)
{
if ((fPitch < 0.6f) && (fPitch >= 0.4f))
{
// 전진
Ojw.CMessage.Write("Forward");
m_nCommand = 1;
MoveForward();
//QWalk(_CNT_WALK, 0, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
else if (fPitch >= 0.6f)
{
// 후진
Ojw.CMessage.Write("Backward");
m_nCommand = 2;
MoveBackward();
//QWalk(_CNT_WALK, 3, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
}
else if (m_ER_Pose == Pose.Fist)
{
if (fPitch >= 0.6f)
{
//if (m_nCommand != 3)
{
Ojw.CMessage.Write("Stand up");
m_nCommand = 3;
//Ojw.CTimer.Wait(1000);
//ActionPlay("80");
//Stop();
MoveTurnLeft();
}
}
else if (fPitch < 0.4f)
{
if (m_nCommand != 4)
{
Ojw.CMessage.Write("have a seat");
m_nCommand = 4;
//Ojw.CTimer.Wait(1000);
//ActionPlay("40");
MoveTurnRight();// Stop();
}
}
else
{
Stop(); // QWalk_Stop(0); // Stop
}
}
else if (m_ER_Pose == Pose.WaveIn)
{
if (m_nCommand != 5)
{
Ojw.CMessage.Write("Right");
m_nCommand = 5;
MoveRight();
//QWalk(_CNT_WALK, 1, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0)); // Right
}
}
else if (m_ER_Pose == Pose.WaveOut)
{
if (m_nCommand != 6)
{
Ojw.CMessage.Write("Left");
m_nCommand = 6;
Move_Up();
//QWalk(_CNT_WALK, 2, ((chkQwm_SpeedUp.Checked == true) ? 1 : 0));
}
}
#endif
//else if (m_ER_Pose == Pose.Rest
}
m_fR_Roll = fRoll;
m_fR_Pitch = fPitch;
m_fR_Yaw = fYaw;
}
else if (e.Myo.Arm == Arm.Right)
{
m_bRight = true;
const float PI = (float)System.Math.PI;
int nDev = 10; //10;
// convert the values to a 0-9 scale (for easier digestion/understanding)
float fRoll = (float)((e.Roll + PI) / (PI * 2.0f) * nDev);
float fPitch = (float)((e.Pitch + PI) / (PI * 2.0f) * nDev);
float fYaw = (float)((e.Yaw + PI) / (PI * 2.0f) * nDev);
//Ojw.CMessage.Write("Roll[{0}], Pitch[{1}], Yaw[{2}]", fRoll, fPitch, fYaw);
if (m_CTId1.Get() >= 100)
{
m_CTId1.Set();
if (m_nFistCommand == 1)
{
int nDir0 = 0x00; // 0x20;
int nDir1 = 0x80; // 0xa0;
int[] anValue = new int[4] {
0x00, 0x00, 0x00, 0x00
};
if (fRoll > 0.6f)
{
int nValue = (int)(fRoll * 10) - 6;
anValue[0] = 0x20;// nDir0 + nValue;
//anValue[1] = nDir1 - nValue;
Move_Left();
//Ojw.CMessage.Write("Left");
}
else if (fRoll < 0.4f)
{
int nValue = 4 - (int)(fRoll * 10) + nDir1;
anValue[0] = 0xa0;// nDir1 + nValue;
//anValue[1] = nDir0 - nValue;
Move_Right();
//Ojw.CMessage.Write("Right");
}
else
{
if (fPitch > 0.6f)
{
anValue[3] = (int)(fPitch * 10) - 6 + nDir0;
Move_Up();
Ojw.CTimer.Wait(100);
Hovering();
//Ojw.CMessage.Write("Up");
}
else if (fPitch < 0.4f)
{
anValue[3] = 0;// 4 - (int)(fPitch * 10) + nDir1;
Hovering();
//Ojw.CMessage.Write("Down");
}
//else Hovering();
}
//else Hovering();
//Send((byte)anValue[0], (byte)anValue[1], (byte)anValue[2], (byte)anValue[3], (byte)0x00);
}
//else Hovering();
}
m_fL_Roll = fRoll;
m_fL_Pitch = fPitch;
m_fL_Yaw = fYaw;
}
}
