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);
}
// same as other control code
bool goStraight(GameObject local, Vector3 remotePos, m3piComm m3piCtrl)
{
Vector3 localPos = local.transform.position;
localPos.y = 0;
Vector3 dis = remotePos - localPos;
print("goStraight\tdis:\t" + dis.magnitude.ToString("F3") + "\tref:\t" +
remotePos.ToString("F3") + "\tcur:\t" + localPos.ToString("F3"));
if (dis.magnitude > (Utility.getInst().disError * 1.5f))
{
Vector3 vCur = local.transform.rotation * Vector3.forward;
float angle = Vector3.Angle(vCur, dis);
bool isForward = (vCur.x * dis.x >= 0) || (vCur.z * dis.z >= 0);
if ((angle > 90.0f) || (angle < -90.0f))
{
isForward = false;
}
else
{
isForward = true;
}
setSpeedWait(dis.magnitude, isForward, m3piCtrl);
return(false);
}
else
{
return(true);
}
}
// Use this for initialization
void Start()
{
m3piCtrls = new m3piComm[2];
m3piCtrls [0] = new m3piComm();
m3piCtrls [0].setName("A");
m3piCtrls [1] = new m3piComm();
m3piCtrls [1].setName("B");
print("create 2 ctrls");
count = new int[2];
count [0] = count [1] = 0;
countNo = 20;
// steps = new int[2];
// steps [0] = steps [1] = 1;
step = 1;
isFirst = new int[2];
isFirst [0] = isFirst [1] = 0;
lastPos = new Vector3[2];
lastPos [0] = lastPos [1] = new Vector3();
defaultRBT = new Vector3[2];
defaultRBT [0] = defaultRBT [1] = new Vector3();
yThreshold = 0.07f;
lastIdx = 1;
m_bRbtConn = false;
carRbts = new GameObject[2];
carRbts [0] = carRbtB; //old
carRbts [1] = carRbtA; //new
print("after start");
}
public bool checkRtnMsg(m3piComm ctrl)
{
// check if there is return msg already
if (!ctrl.m_bRtn)
{
float executeTime = Time.time - ctrl.m_runTime;
// check if it is already too long then return and sync up them again
if (executeTime < (ctrl.m_cmdTime + 0.5f))
{
return(false);
}
Debug.Log("exe:\t" + executeTime + "\test:\t" + ctrl.m_cmdTime);
Debug.Log("wait too long:\t" + executeTime);
ctrl.m_exStop = true;
return(true);
}
if (ctrl.m_returnMsg.Length > 0)
{
Debug.Log(ctrl.m_returnMsg);
}
ctrl.m_returnMsg = "";
if (StreamSingleton.getInst().getReceiveThread() != null)
{
//Debug.Log ("abort in checkRtnMsg and return true" + ctrl.receiveThread.ThreadState);
StreamSingleton.getInst().getReceiveThread().Abort();
}
return(true);
}
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);
}
static public m3piComm getInst()
{
if (m_inst == null)
{
m_inst = new m3piComm();
}
return(m_inst);
}
void createM3pi()
{
m3piCtrls = new m3piComm[2];
m3piCtrls [0] = new m3piComm();
m3piCtrls [0].setName("A");
m3piCtrls [1] = new m3piComm();
m3piCtrls [1].setName("B");
print("create 2 ctrls");
}
// Use this for initialization
void Start()
{
m_inst = m3piComm.getInst();
m_inst.open();
m_inst.setName(robotName);
rot = this.transform.rotation;
pos = this.transform.position;
//Debug.Log (pos);
}
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);
}
// Use this for initialization
void Start()
{
_destination = new Vector3();
if (!_isViewer)
{
_robotCtrl = new m3piComm();
_robotCtrl.setName("B");
}
step = 1;
wCtrl = stylus.GetComponent <wandCtrl> ();
}
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);
}
// Use this for initialization
void Start()
{
m3piCtrls = new m3piComm[2];
m3piCtrls [0] = new m3piComm();
m3piCtrls [0].setName("A");
m3piCtrls [1] = new m3piComm();
m3piCtrls [1].setName("B");
count = new int[2];
count [0] = count [1] = 0;
countNo = 20;
step = new int[2];
step [0] = step [1] = 0;
}
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);
}
protected bool turnAround(GameObject local, GameObject remote, m3piComm m3piCtrl, bool isLocal = false)
{
Vector3 vFacing, vCur;
if (isLocal)
{
vFacing = remote.transform.localPosition - local.transform.localPosition;
vCur = local.transform.localRotation * Vector3.forward;
}
else
{
vFacing = remote.transform.position - local.transform.position;
vCur = local.transform.rotation * Vector3.forward;
}
vCur.y = 0;
vFacing.y = 0;
float angle = Vector3.Angle(vCur, vFacing);
if (angle > 90.0f)
{
angle = angle - 180.0f;
}
if (Mathf.Abs(angle) % 180.0f > Utility.getInst().angleError)
{
Vector3 vUp = Vector3.Cross(vCur, vFacing);
// print ("turnAround:\t" + angle + "\tupVector:\t" + vUp.ToString ("F2"));
if (vUp.y > 0.00005)
{
setAngle(false, angle, m3piCtrl);
}
else if (vUp.y < -0.00005)
{
setAngle(true, angle, m3piCtrl);
}
else
{
return(true);
}
return(false);
}
else
{
return(true);
}
}
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;
}
}
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;
}
}
protected bool goStraight(GameObject local, GameObject remote, m3piComm m3piCtrl, bool isLocal = false)
{
Vector3 localPos, remotePos;
if (isLocal)
{
localPos = local.transform.localPosition;
remotePos = remote.transform.localPosition;
}
else
{
localPos = local.transform.position;
remotePos = remote.transform.position;
}
localPos.y = 0;
remotePos.y = 0;
Vector3 dis = remotePos - localPos;
// print ("goStraight\tdis:\t" + dis.magnitude.ToString ("F3") + "\tref:\t" +
// remotePos.ToString ("F3") + "\tcur:\t" + localPos.ToString ("F3"));
if (dis.magnitude > Utility.getInst().disError)
{
Vector3 vCur = local.transform.rotation * Vector3.forward;
//Vector3 vUp = Vector3.Cross (dis, vCur);//print ("goStraight\tvCur:\t" + vCur.ToString ("F2") + "\tvUp:\t" + vUp.ToString ("F2"));
float angle = Vector3.Angle(vCur, dis);
bool isForward = (vCur.x * dis.x >= 0) || (vCur.z * dis.z >= 0);
if ((angle > 90.0f) || (angle < -90.0f))
{
isForward = false;
}
else
{
isForward = true;
}
setSpeedWait(dis.magnitude, isForward, m3piCtrl);
return(false);
}
else
{
return(true);
}
}
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);
}
// Use this for initialization
void Start()
{
m3piCtrler = new m3piComm();
m3piCtrler.setName(robotName);
count = 0;
step = 1;
countNo = 40;
// initialize
isLastRound = false;
isLastStraight = false;
lastPos = new Vector3();
lastRot = Quaternion.identity;
yThreshold = 0.07f;
//sync (carB2, carA2);
carB2ref.SetActive(false);
//enableTest = true;
}
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);
}
// new version for sharing thread
public bool checkRtnMsg2(m3piComm ctrl)
{
//if (ctrl.m_runTime < 0.1f || ctrl.m_bRtn)
if (ctrl.m_bRtn)
{
return(true); // just started
}
// check if the command is matched with one of the received msg and will discard unused msg
int matchResult = StreamSingleton.getInst().match(ctrl.m_command);
if (matchResult == 0)
{
// matched
StreamSingleton.getInst().minusThread();
ctrl.m_bRtn = true;
return(true);
}
// then check if it waits not too long
else //(matchResult == 2)
{
float executeTime = Time.time - ctrl.m_runTime;
if (executeTime < (ctrl.m_cmdTime + 0.8f))
{
return(false);
}
else
{
// too long then stop one, if it is 0 for link count, kill the thread
Debug.Log("wait too long:\t" + ctrl.m_cmdTime + "\t" + executeTime);
//StreamSingleton.getInst ().setExstop (true);
StreamSingleton.getInst().minusThread();
ctrl.m_bRtn = true; // do not need to wait for next return msg
return(true);
}
}
//return false;
}
bool turnAround(GameObject local, Vector3 remotePos, m3piComm m3piCtrl)
{
Vector3 vFacing = remotePos - local.transform.position;
Vector3 vCur = local.transform.rotation * Vector3.forward;
vCur.y = 0;
vFacing.y = 0;
float angle = Vector3.Angle(vCur, vFacing);
if (angle > 90.0f)
{
angle = angle - 180.0f;
}
if (Mathf.Abs(angle) % 180.0f > 8.0f)
{
Vector3 vUp = Vector3.Cross(vCur, vFacing);
print("turnAround:\t" + angle + "\tupVector:\t" + vUp.ToString("F2"));
if (vUp.y > 0.00005)
{
setAngle(false, angle, m3piCtrl);
}
else if (vUp.y < -0.00005)
{
setAngle(true, angle, m3piCtrl);
}
else
{
return(true);
}
return(false);
}
else
{
return(true);
}
}
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);
}
}
// 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 createM3pi()
{
m3piCtrl = new m3piComm();
m3piCtrl.setName("B");
print("create ctrl");
}
