public IEnumerator DirectionalMoving()
{
rc.aims.Insert(0, telega.PointA.transform.position);
rc.aims.Insert(0, telega.PointA.transform.position + telega.A - telega.B);
for (int i = 0; i < rc.aims.Count; ++i)
{
rc.aims[i] = new Vector3(rc.aims[i].x, telega.transform.position.y, rc.aims[i].z);
}
for (int j = 1; j < rc.aims.Count - 1; ++j)
{
a = rc.aims[j] - rc.aims[j - 1];
b = rc.aims[j + 1] - rc.aims[j];
angle = new float[3] {
30f, -30f, 90f
};
float alpha = Mathf.Acos(Vector3.Dot(a.normalized, b.normalized)) * Mathf.Rad2Deg;
dist = new float[3] {
Mathf.Abs(Mathf.PI * 20f * alpha / 180f), Mathf.Abs(Mathf.PI * 20f * alpha / 180f), Mathf.Abs(Mathf.PI * 20f * alpha / 180f)
};
if (Vector3.Cross(a, b).y >= 0)
{
isReversed = new float[3] {
-1, -1, 1
};
}
if (Vector3.Cross(a, b).y < 0)
{
isReversed = new float[3] {
1, 1, -1
};
}
//SceneManager.Net.Sender(RobotCommands.TelegaMoving());
var aimsRobot = CoordTransformation.UnityToRobotPosOnly(new Vector4(rc.aims[j].x, rc.aims[j].y, rc.aims[j].z, 1));
SceneManager.Net.Sender(RobotCommands.TelegaMoving(aimsRobot[0].ToString("0.00"), aimsRobot[1].ToString("0.00")));
yield return(new WaitForSeconds(0.5f));
GameObject.Find("telega").GetComponent <telegaScript>().isMoved = true;
float dst = Mathf.Sqrt((rc.aims[j + 1].x - rc.aims[j].x) * (rc.aims[j + 1].x - rc.aims[j].x) +
(rc.aims[j + 1].z - rc.aims[j].z) * (rc.aims[j + 1].z - rc.aims[j].z)) * 1000f;
dist = new float[3] {
dst, dst, dst
};
isReversed[0] = isReversed[1] = isReversed[2] = 1;
angle[0] = angle[1] = angle[2] = 0;
yield return(new WaitForSeconds(0.5f));
GameObject.Find("telega").GetComponent <telegaScript>().isMoved = true;
}
rc.aims.RemoveAt(0);
rc.aims.RemoveAt(0);
}
public static void AddFromObserver(float x, float y)
{
raycast cam = GameObject.FindObjectOfType <raycast>();
cam.aims.Add(CoordTransformation.RobotToUnityPosOnly(new Vector3(x, y, 381f)));
cam.balls.Add(Instantiate(cam.GetComponent <raycast>().BallPr));
cam.balls[cam.GetComponent <raycast>().balls.Count - 1].transform.position = cam.GetComponent <raycast>().aims[cam.GetComponent <raycast>().balls.Count - 1];
}
// Update is called once per frame
void Update()
{
if (!SceneManager.telega.telega.isMoved)
{
if (Input.GetMouseButton(0))
{
ray = cam.ScreenPointToRay(Input.mousePosition);
if ((Physics.Raycast(ray, out hit)) &&
(UnityEngine.EventSystems.EventSystem.current.IsPointerOverGameObject() == false ||
hit.collider.gameObject.name == "BallSpirit") &&
(hit.collider.gameObject.name == "Plane"))
{
BallSpirit.transform.position = new Vector3(hit.point.x, point.transform.position.y, hit.point.z);
BallSpirit.GetComponent <MeshRenderer>().enabled = true;
// point.transform.position = new Vector3(hit.point.x, point.transform.position.y, hit.point.z);
}
else
{
BallSpirit.GetComponent <MeshRenderer>().enabled = false;
}
}
if (Input.GetMouseButtonUp(0))
{
ray = cam.ScreenPointToRay(Input.mousePosition);
//Physics.RaycastAll(ray);
if ((UnityEngine.EventSystems.EventSystem.current.IsPointerOverGameObject() == false) &&
(Physics.Raycast(ray, out hit)))
{
BallSpirit.GetComponent <MeshRenderer>().enabled = false;
aims.Add(new Vector3(hit.point.x, point.transform.position.y, hit.point.z));
balls.Add(Instantiate(BallPr, aims[aims.Count - 1], Quaternion.identity));
if (balls.Count >= 10)
{
GameObject.Find("PointList").GetComponent <ContentSizeFitter>().enabled = true;
StartCoroutine("DropScrollbar");
}
}
}
if (balls.Count < 10)
{
GameObject.Find("PointList").GetComponent <ContentSizeFitter>().enabled = false;
GameObject.Find("PointList").transform.position = PointList.transform.parent.position;
GameObject.Find("PointList").GetComponent <RectTransform>().sizeDelta = PointList.transform.parent.GetComponent <RectTransform>().sizeDelta;
}
}
PointList.text = "";
for (int j = 0; j <= aims.Count - 1; ++j)
{
var aimsRobot = CoordTransformation.UnityToRobotPosOnly(new Vector4(aims[j].x, aims[j].y, aims[j].z, 1));
PointList.text += aimsRobot[0].ToString("0.00") + " " + aimsRobot[1].ToString("0.00") + "\r\n";
}
}
public IEnumerator ParallelMoving()
{
rc.aims.Insert(0, telega.PointA.transform.position);
rc.aims.Insert(0, telega.PointA.transform.position + telega.A - telega.B);
for (int i = 0; i < rc.aims.Count; ++i)
{
rc.aims[i] = new Vector3(rc.aims[i].x, telega.transform.position.y, rc.aims[i].z);
}
for (int j = 1; j < rc.aims.Count - 1; ++j)
{
a = rc.aims[j] - rc.aims[j - 1];
b = rc.aims[j + 1] - rc.aims[j];
for (int i = 0; i < 3; ++i)
{
angle[i] = Mathf.Acos(Vector3.Dot(a.normalized, b.normalized)) * Mathf.Rad2Deg;
if (angle[i] > 180f)
{
angle[i] = angle[i] - 360f;
}
if (angle[i] > 120f)
{
angle[i] -= 180f;
isReversed[i] = -1f;
}
else if (angle[i] < -120f)
{
angle[i] += 180f;
isReversed[i] = -1f;
}
else
{
isReversed[i] = 1f;
}
dist[i] = Mathf.Sqrt((rc.aims[j + 1].x - rc.aims[j].x) * (rc.aims[j + 1].x - rc.aims[j].x) +
(rc.aims[j + 1].z - rc.aims[j].z) * (rc.aims[j + 1].z - rc.aims[j].z)) * 1000f;
}
//SceneManager.Net.Sender(RobotCommands.TelegaMoving());
var aimsRobot = CoordTransformation.UnityToRobotPosOnly(new Vector4(rc.aims[j].x, rc.aims[j].y, rc.aims[j].z, 1));
//SceneManager.Net.Sender(RobotCommands.TelegaMoving(aimsRobot[0].ToString("0.00"), aimsRobot[1].ToString("0.00")));
yield return(new WaitForSeconds(0.5f));
GameObject.Find("telega").GetComponent <telegaScript>().isMoved = true;
}
rc.aims.RemoveAt(0);
rc.aims.RemoveAt(0);
}
void FixedUpdate()
{
A = PointA.transform.position;
B = PointB.transform.position;
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
C = Camera.GetComponent <raycast>().aims[0];
C.y = teleg.transform.position.y;
}
else
{
C = A;
C.y = teleg.transform.position.y;
}
a = B - A;
b = C - A;
if (Math.Abs(A.x - C.x) < step && (Math.Abs(A.z - C.z) < step))
{
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
isMoved = true;
Camera.GetComponent <raycast>().aims.RemoveAt(0);
Destroy(Camera.GetComponent <raycast>().balls[0]);
Camera.GetComponent <raycast>().balls.RemoveAt(0);
if (Camera.GetComponent <raycast>().aims.Count > 0)
{
C = Camera.GetComponent <raycast>().aims[0];
C.y = teleg.transform.position.y;
}
}
else
{
isMoved = false;
}
}
switch (Type)
{
case MoveType.RotateTeleguAndMove:
Directional();
break;
case MoveType.RotateBarsAndMove:
Parallel();
break;
}
float[] coord = FanucModel.GetCoordsFromMat(CoordTransformation.UnityToRobot(FanucModel.coordMatrixDegrees(teleg.transform.position, teleg.transform.eulerAngles)));
curPosText.text = coord[0].ToString("0.00") + " " + coord[1].ToString("0.00") + " " + coord[5].ToString("0.00");
}
public void Add()
{
if (!SceneManager.telega.telega.isMoved)
{
try
{
if (input.text.Contains(","))
{
throw new System.Exception();
}
if (input.text.Length > 0)
{
var arr = input.text.Split();
if (arr.Length == 2)
{
raycast cam = GameObject.FindObjectOfType <raycast>();
//var aimsRobot = CoordTransformation.UnityToRobotPosOnly(new Vector4(aims[j].x, aims[j].y, aims[j].z, 1));
cam.aims.Add(CoordTransformation.RobotToUnityPosOnly(new Vector3(float.Parse(arr[0]), float.Parse(arr[1]), 381f)));
cam.balls.Add(Instantiate(cam.GetComponent <raycast>().BallPr));
cam.balls[cam.GetComponent <raycast>().balls.Count - 1].transform.position = cam.GetComponent <raycast>().aims[cam.GetComponent <raycast>().balls.Count - 1];
input.text = "";
if (Camera.GetComponent <raycast>().balls.Count >= 10)
{
PointList.GetComponent <ContentSizeFitter>().enabled = true;
// GameObject.Find("PointList").transform.position = new Vector3(GameObject.Find("PointList").transform.position.x, GameObject.Find("RectObject").transform.position.y, GameObject.Find("PointList").transform.position.z);
ScrollBar.GetComponent <Scrollbar>().value = 0.00000f;
}
}
else
{
throw new System.Exception();
}
}
else
{
throw new System.Exception();
};
}
catch (System.Exception)
{
Debug.Log("Wrong string");
input.text = "Wrong string. Try again";
}
}
}
// Use this for initialization
void Start()
{
isMoved = false;
rotstep = WheelAngleSpeed * WheelRadius / telegaRadius;
Debug.Log(rotstep);
//rotstep = 1e-3f;
step = WheelAngleSpeed * WheelRadius * Mathf.Deg2Rad;
C = PointA.transform.position;
//Net = SceneManager.Net;
//Net.gameObject.SetActive(true);
Camera = GameObject.Find("Camera");
modeText = GameObject.Find("Telega Mode Text").GetComponent <Text>();
curPosText = GameObject.Find("Telega currnet position").GetComponent <Text>();
Debug.Log(teleg.transform.position + " " + teleg.transform.eulerAngles);
var res = FanucModel.GetCoordsFromMat(CoordTransformation.UnityToRobot(FanucModel.coordMatrixDegrees(teleg.transform.position, teleg.transform.eulerAngles)));
Debug.Log(res[0] + " " + res[1] + " " + res[2] + " " + res[3] + " " + res[4] + " " + res[5]);
modeText.text = "Directional";
}
void DropdownValueChanged(Dropdown change)
{
var obj = SceneManager.Pull.Find(change.value);
//Debug.Log("ROT: " + obj.transform.rotation.eulerAngles);
//Vector4 coordsPos = new Vector4 ( obj.transform.position[0], obj.transform.position[1], obj.transform.position[2], 1 );
Debug.Log(obj.transform.eulerAngles);
Matrix4x4 coords = FanucModel.rotMatrixDegrees(obj.transform.eulerAngles.x, obj.transform.eulerAngles.y, obj.transform.eulerAngles.z);
coords[0, 3] = obj.transform.position[0];
coords[1, 3] = obj.transform.position[1];
coords[2, 3] = obj.transform.position[2];
coords = CoordTransformation.UnityToRobot(coords);
var outCoords = FanucModel.GetCoordsFromMat(coords);
outCoordsStr = outCoords[0] + " " + outCoords[1] + " " + (outCoords[2] + 190f) + " " + outCoords[3] + " "
+ outCoords[4] + " " + outCoords[5];
SceneManager.FanucSettingsPanel.coordField.text = outCoordsStr;
//coordText.text = coordTrans.UnityToRobotPosOnly(coordsPos).ToString();
}
// Use this for initialization
public void Syncronization(string message)//string ObjectList)
{
//-------------------------------------Here should be parser--------------------------------------------------------
//string message = "{\"fanuc\":\"12 32 1 34 65 -90\",\"telega\":\"100 -2000 300 -1 -2 -3\"}";
//Debug.Log("synchro...");
try
{
var dict = JsonConvert.DeserializeObject <Dictionary <string, string> >(message);
// Debug.Log(dict);
NumberofObjects = dict.Count; //what's the magic number???
//Debug.Log(NumberofObjects);
Names = new string[NumberofObjects]; //IDs
PositionCoords = new Vector3[NumberofObjects];
//Quaternion[] RotationCoords = new Quaternion[NumberofObjects];
int index = 0;
foreach (var i in dict.Keys)
{
Debug.Log(i + ": " + dict[i]);
Names[index] = i;
StringCoordArr = dict[i].Split();
floatCoordArr = new float[StringCoordArr.Length];
//switch (i)
//{
// case "fanuc":
// {
// FanucCoords = floatCoordArr;
// break;
// }
// case "telega":
// {
// Debug.Log("here telega " + StringCoordArr);
// TelegaCoord = floatCoordArr;
// Debug.Log(floatCoordArr[0]+" "+ floatCoordArr[1]);
// break;
// }
//}
if (i == "fanuc")
{
FanucCoords = floatCoordArr;
}
if (i == "telega")
{
TelegaCoord[0] = float.Parse(StringCoordArr[0]);
TelegaCoord[1] = float.Parse(StringCoordArr[1]);
}
else
if (SceneManager.Pull.Find(Names[index]))
{
for (int j = 0; j < StringCoordArr.Length; ++j)
{
floatCoordArr[j] = float.Parse(StringCoordArr[j]);
}
PositionCoords[index] = CoordTransformation.RobotToUnityPosOnly(new Vector4(float.Parse(StringCoordArr[0]), float.Parse(StringCoordArr[1]), float.Parse(StringCoordArr[2]) - 190f, 1));
Debug.Log(Names[index] + " object found, coords added");
}
// Debug.Log("length of parsed array " + StringCoordArr.Length);
//Debug.Log("Position: "+PositionCoords[index]);
//Debug.Log(floatCoordArr);
++index;
}
//-------------------------------------Here he ends------------------------------------------------------------------
Synchro = true;
}
catch (UnityException ex)
{
Debug.Log(ex);
}
}