void setAngle(bool lft, float angle, m3piComm m3piCtrl)
{
if (angle < 0)
{
lft = !lft;
}
angle = Mathf.Abs(angle);
// setAngleHelp (m3piCtrl, ref angle, 21.0f, 6, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 15.0f, 4, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 8.0f, 1, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 51.0f, 15, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 35.0f, 10, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 28f, 8, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 20f, 6, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 16f, 5, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 8f, 3, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 3.4f, 2, 2, lft);
for (int i = 0; i < m3piCtrl.angleHelpArray.Length; i++)
{
setAngleHelp(m3piCtrl, ref angle,
m3piCtrl.angleHelpArray [i].angle, m3piCtrl.angleHelpArray [i].sp, m3piCtrl.angleHelpArray [i].wt, lft);
}
m3piCtrl.run2(Time.time);
}
void setAngle(bool lft, float angle, m3piComm m3piCtrl)
{
if (angle < 0)
{
lft = !lft;
}
angle = Mathf.Abs(angle);
for (int i = 0; i < m3piCtrl.angleHelpArray.Length; i++)
{
setAngleHelp(m3piCtrl, ref angle, m3piCtrl.angleHelpArray [i].angle, m3piCtrl.angleHelpArray [i].sp, m3piCtrl.angleHelpArray [i].wt, lft);
}
// setAngleHelp (m3piCtrl, ref angle, 55.0f, 5, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 44.0f, 3, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 34.7f, 2, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 24.7f, 5, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 17.98f, 3, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 9.6f, 1, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 5.0f, 0, 1, lft);
// setAngleHelp (m3piCtrl, ref angle, 51.0f, 15, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 35.0f, 10, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 28f, 8, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 20f, 6, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 16f, 5, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 8f, 3, 2, lft);
// setAngleHelp (m3piCtrl, ref angle, 3.4f, 2, 2, lft);
m3piCtrl.run2(Time.time);
//m_returnMsg = m3piCtrlB.m_returnMsg;
//Debug.Log ("receive from m3pi:\t" + m_returnMsg);
}
void setSpeedWait(float dis, bool fw, m3piComm m3piCtrl)
{
for (int i = 0; i < m3piCtrl.posHelpArray.Length; i++)
{
setSpeedWaitHelp(m3piCtrl, ref dis,
m3piCtrl.posHelpArray [i].dis, m3piCtrl.posHelpArray [i].sp, m3piCtrl.posHelpArray [i].wt, fw);
}
m3piCtrl.run2(Time.time);
}
void setSpeedWait(float dis, bool fw, m3piComm m3piCtrl)
{
for (int i = 0; i < m3piCtrl.posHelpArray.Length; i++)
{
setSpeedWaitHelp(m3piCtrl, ref dis,
m3piCtrl.posHelpArray[i].dis, m3piCtrl.posHelpArray[i].sp, m3piCtrl.posHelpArray[i].wt, fw);
}
m3piCtrl.run2(Time.time);
//m_returnMsg = m3piCtrlB.m_returnMsg;
//Debug.Log ("receive from m3pi:\t" + m_returnMsg);
}
void setAngle(bool lft, float angle, m3piComm m3piCtrl)
{
if (angle < 0)
{
lft = !lft;
}
angle = Mathf.Abs(angle);
for (int i = 0; i < m3piCtrl.angleHelpArray.Length; i++)
{
setAngleHelp(m3piCtrl, ref angle,
m3piCtrl.angleHelpArray [i].angle, m3piCtrl.angleHelpArray [i].sp, m3piCtrl.angleHelpArray [i].wt, lft);
}
m3piCtrl.run2(Time.time);
}
void setAngle(bool lft, float angle, m3piComm m3piCtrl)
{
if (angle < 0)
{
lft = !lft;
}
angle = Mathf.Abs(angle);
for (int i = 0; i < m3piCtrl.angleHelpArray.Length; i++)
{
setAngleHelp(m3piCtrl, ref angle,
m3piCtrl.angleHelpArray[i].angle, m3piCtrl.angleHelpArray[i].sp, m3piCtrl.angleHelpArray[i].wt, lft);
}
m3piCtrl.run2(Time.time);
//m_returnMsg = m3piCtrlB.m_returnMsg;
//Debug.Log ("receive from m3pi:\t" + m_returnMsg);
}
void setSpeedWait(float dis, bool fw, m3piComm m3piCtrl)
{
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.25f, 7, 8, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.13f, 6, 6, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.06f, 4, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.022f, 3, 2, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.19f, 25, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.167f, 20, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.126f, 15, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.093f, 10, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.06f, 6, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.033f, 3, 3, fw);
for (int i = 0; i < m3piCtrl.posHelpArray.Length; i++)
{
setSpeedWaitHelp(m3piCtrl, ref dis,
m3piCtrl.posHelpArray [i].dis, m3piCtrl.posHelpArray [i].sp, m3piCtrl.posHelpArray [i].wt, fw);
}
m3piCtrl.run2(Time.time);
}
void setSpeedWait(float dis, bool fw, m3piComm m3piCtrl)
{
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.21f, 20, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.17f, 15, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.11f, 15, 2, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.074f, 10, 2, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.04f, 5, 2, fw);
for (int i = 0; i < m3piCtrl.posHelpArray.Length; i++)
{
setSpeedWaitHelp(m3piCtrl, ref dis, m3piCtrl.posHelpArray [i].dis, m3piCtrl.posHelpArray [i].sp, m3piCtrl.posHelpArray [i].wt, fw);
}
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.19f, 25, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.167f, 20, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.126f, 15, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.093f, 10, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.06f, 6, 3, fw);
// setSpeedWaitHelp (m3piCtrl, ref dis, 0.033f, 3, 3, fw);
m3piCtrl.run2(Time.time);
//m_returnMsg = m3piCtrlB.m_returnMsg;
//Debug.Log ("receive from m3pi:\t" + m_returnMsg);
}
// forward 0.05 0.025 0.02
// left 15 6 3
// Update is called once per frame
void Update()
{
if (Input.GetKeyDown(KeyCode.F))
{
//m_inst.forwardTest (roundTest);
if (Utility.getInst().checkRtnMsg2(m_inst))
{
m_inst.clear();
m_inst.setSpeed(speed);
m_inst.setWaitTime(waitTime);
m_inst.forward();
m_inst.run2();
Debug.Log("speed:\t" + speed + "\twait:\t" + waitTime);
Debug.Log(Vector3.Distance(pos, this.transform.position).ToString("F8"));
Debug.Log("rot:" + Quaternion.Angle(rot, this.transform.rotation).ToString("F8"));
rot = this.transform.rotation;
pos = this.transform.position;
}
else
{
print("busy");
}
}
if (Input.GetKeyDown(KeyCode.B))
{
// m_inst.backwardTest (roundTest);
// Debug.Log (roundTest);
if (Utility.getInst().checkRtnMsg2(m_inst))
{
m_inst.clear();
m_inst.setSpeed(speed);
m_inst.setWaitTime(waitTime);
m_inst.backward();
m_inst.run2();
Debug.Log("speed:\t" + speed + "\twait:\t" + waitTime);
Debug.Log(Vector3.Distance(pos, this.transform.position).ToString("F8"));
Debug.Log("rot:" + Quaternion.Angle(rot, this.transform.rotation).ToString("F8"));
rot = this.transform.rotation;
pos = this.transform.position;
}
}
if (Input.GetKeyDown(KeyCode.L))
{
if (Utility.getInst().checkRtnMsg2(m_inst))
{
m_inst.clear();
m_inst.setSpeed(speed);
m_inst.setWaitTime(waitTime);
m_inst.left();
m_inst.run2(Time.time);
// // test threads
// Thread receiveThread = new Thread (m_inst.receive);
// receiveThread.Start ();
Debug.Log("speed:\t" + speed + "\twait:\t" + waitTime);
Debug.Log(Quaternion.Angle(rot, this.transform.rotation).ToString("F8"));
pos = this.transform.position;
rot = this.transform.rotation;
}
else
{
print("busy");
}
}
if (Input.GetKeyDown(KeyCode.R))
{
if (Utility.getInst().checkRtnMsg2(m_inst))
{
m_inst.clear();
m_inst.setSpeed(speed);
m_inst.setWaitTime(waitTime);
m_inst.right();
m_inst.run2();
Debug.Log("speed:\t" + speed + "\twait:\t" + waitTime);
Debug.Log(Quaternion.Angle(rot, this.transform.rotation).ToString("F8"));
pos = this.transform.position;
rot = this.transform.rotation;
}
}
if (Input.GetKeyDown(KeyCode.S))
{
m_inst.stop();
m_inst.run2();
//Debug.Log ("speed:\t" + speed + "\twait:\t" + waitTime);
//Debug.Log(Quaternion.Angle(rot,this.transform.rotation).ToString("F8"));
//pos = this.transform.position;
//rot = this.transform.rotation;
}
if (Input.GetKeyDown(KeyCode.P))
{
Debug.Log("speed:\t" + speed + "\twait:\t" + waitTime);
Debug.Log("pos:\t" + this.transform.position.ToString("F8"));
Debug.Log("rot:\t" + this.transform.rotation.ToString("F8"));
}
// if (Input.GetKeyDown (KeyCode.Z)) {
// m_inst.left ();
// Debug.Log (roundTest);
// //Debug.Log (Vector3.Distance(pos,this.transform.position).ToString("F8"));
// Debug.Log(Quaternion.Angle(rot,this.transform.rotation).ToString("F8"));
// pos = this.transform.position;
// rot = this.transform.rotation;
// }
// if (Input.GetKeyDown (KeyCode.Y)) {
// m_inst.right ();
// Debug.Log (roundTest);
// //Debug.Log (Vector3.Distance(pos,this.transform.position).ToString("F8"));
// Debug.Log(Quaternion.Angle(rot,this.transform.rotation).ToString("F8"));
// pos = this.transform.position;
// rot = this.transform.rotation;
// }
// if (Input.GetKeyDown (KeyCode.H)) {
// m_inst.high ();
// Debug.Log ("high");
// }
// if (Input.GetKeyDown (KeyCode.M)) {
// m_inst.median ();
// Debug.Log ("median");
// }
// if (Input.GetKeyDown (KeyCode.L)) {
// m_inst.low ();
// Debug.Log ("low");
// }
// test receive data
if (m_inst.m_bRtn)
{
if (m_inst.m_returnMsg.Length > 0)
{
print(m_inst.m_returnMsg);
}
m_inst.m_returnMsg = "";
}
}