void setAngleHelp(m3piComm m3piCtrl, ref float angle, float thres, int sp, int wt, bool lft)
{
while (angle > thres)
{
m3piCtrl.setSpeed(sp);
m3piCtrl.setWaitTime(wt);
if (lft)
{
m3piCtrl.left();
}
else
{
m3piCtrl.right();
}
angle -= thres;
}
}
void setSpeedWaitHelp(m3piComm m3piCtrl, ref float dis, float thres, int sp, int wt, bool fw)
{
while (dis > thres)
{
m3piCtrl.setSpeed(sp);
m3piCtrl.setWaitTime(wt);
if (fw)
{
m3piCtrl.forward();
}
else
{
m3piCtrl.backward();
}
dis -= thres;
}
}
// protected void ignoreYPos (GameObject local, GameObject remote, ref Vector3 localPos, ref Vector3 remotePos)
// {
// // get local position
// localPos = local.transform.localPosition;
// remotePos = remote.transform.localPosition;
// // ignore y information
// localPos.y = 0;
// remotePos.y = 0;
// }
//
// protected void sync (GameObject local, GameObject remote, m3piComm m3piCtrl, ref int step, int index = 0)
// {
// if (!Utility.getInst ().checkRtnMsg2 (m3piCtrl))
// return;
//
// // print ("local\t" + local.transform.position + "\t" + local.transform.localPosition);
// // print ("remote\t" + remote.transform.position + "\t" + remote.transform.localPosition);
// Utility.getInst ().drawRays (local.transform, remote.transform, true);
//
// Vector3 localPos = new Vector3 (), remotePos = new Vector3 ();
// ignoreYPos (local, remote, ref localPos, ref remotePos);
//
// m3piCtrl.clear ();
//
// // send command
// if (step != 0) {
// print ("move index:\t" + index + "\tstep:\t" + step);
// switch (step) {
// case 1:
// // check distance first
// if (Utility.getInst ().checkMatchV2 (localPos, remotePos)) {
// step = 0;
// } else {
// if (turnAround (local, remote, m3piCtrl, true)) {
// // print ("move index:\t" + index + "\tstep:\t" + step);
// goStraight (local, remote, m3piCtrl, true);
// step = 2;
// }
// }
// break;
// case 2:
// // moved car with going straight
// if (goStraight (local, remote, m3piCtrl, true)) {
// step = 0;
// //print ("move index:\t" + index + "\tstep:\t" + step);
// } else {
// step = 1;
// //print ("move index:\t" + index + "\tstep:\t" + step);
// }
// break;
// default:
// break;
// }
// }
// }
void setAngleHelp(m3piComm m3piCtrl, ref float angle, float thres, int sp, int wt, ref bool lft, bool abs = false)
{
//while (angle > thres) {
while ((!abs && (angle > thres)) ||
(abs && (Mathf.Abs(angle - thres) < angle)))
{
m3piCtrl.setSpeed(sp);
m3piCtrl.setWaitTime(wt);
if (lft)
{
m3piCtrl.left();
}
else
{
m3piCtrl.right();
}
angle -= thres;
if (angle < 0)
{
lft = !lft;
}
angle = Mathf.Abs(angle);
}
}
void setSpeedWaitHelp(m3piComm m3piCtrl, ref float dis, float thres, int sp, int wt, ref bool fw, bool abs = false)
{
while ((!abs && (dis > thres)) ||
(abs && (Mathf.Abs(dis - thres) < dis)))
{
// while (dis > thres) {
m3piCtrl.setSpeed(sp);
m3piCtrl.setWaitTime(wt);
if (fw)
{
m3piCtrl.forward();
}
else
{
m3piCtrl.backward();
}
dis -= thres;
if (dis < 0)
{
fw = !fw;
}
dis = Mathf.Abs(dis);
}
}
// 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 = "";
}
}