protected IEnumerator ManualWork() { while (workMode == WorkMode.Manual) { //yield return new WaitForSeconds(0.1f); yield return(null); // записываем актуальное расчитанное положение джоинтов Joint6 actualJointVector = scenceControl.GetOptionJoint(); int atualSpeed = scenceControl.targetProperties.speed; // отправляем изменённое положение джоинтов if (jointVector != actualJointVector) { jointVector = actualJointVector; protocol.SendRequest(actualJointVector.ToByteSteam(), "J"); } // отправляем изменёнyю скорость if (speed != atualSpeed) { speed = atualSpeed; protocol.SendRequest(BitConverter.GetBytes((ushort)speed), "S"); } } }
static public Vector5 ForwardKinematicStatic(Joint6 inpJoint) { // Inp J1, J2 ... // Out x,y,z, a,b,c float l1 = 7, l2 = 5, l3 = 6.7f, l4 = 3; Joint6 localInpJoint = inpJoint * Mathf.Deg2Rad; float xz = l2 * Mathf.Sin(localInpJoint.J2) + l3 * Mathf.Sin(localInpJoint.J2 + localInpJoint.J3) + l4 * Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4); float x = Mathf.Sin(localInpJoint.J1) * xz; float z = Mathf.Cos(localInpJoint.J1) * xz; float y = l1 + l2 * Mathf.Cos(localInpJoint.J2) + l3 * Mathf.Cos(localInpJoint.J2 + localInpJoint.J3) + l4 * Mathf.Cos(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4); Vector3 orientVector = new Vector3(Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4) * Mathf.Sin(localInpJoint.J1), Mathf.Cos(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4), Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4) * Mathf.Cos(localInpJoint.J1)); Vector2 orientVectorP = new Vector2(Mathf.Sqrt(Mathf.Pow(orientVector.x, 2) + Mathf.Pow(orientVector.z, 2)) * Mathf.Sign(orientVector.x + orientVector.z), orientVector.y); if (inpJoint.J1 < -45 || inpJoint.J1 > 135) { orientVectorP.x = -orientVectorP.x; } float angleA = Vector2.SignedAngle(Vector2.right, orientVectorP); float angleB = inpJoint.J5; return(new Vector5(x, y, z, angleA, angleB, inpJoint.J6)); }
private void JointMove(Joint6 jointVar, GameObject[] joints) { joints[0].transform.localRotation = Quaternion.Euler(0, jointVar.J1, 0); joints[1].transform.localRotation = Quaternion.Euler(jointVar.J2, 0, 0); joints[2].transform.localRotation = Quaternion.Euler(jointVar.J3, 0, 0); joints[3].transform.localRotation = Quaternion.Euler(jointVar.J4, 0, 0); joints[4].transform.localRotation = Quaternion.Euler(0, jointVar.J5, 0); }
static protected Vector3 VectorZ(Joint6 InpJoint) // вектор вдоль последнего звена { float r13 = Mathf.Sin(InpJoint.J1) * Mathf.Sin(InpJoint.J2 + InpJoint.J3 + InpJoint.J4); float r23 = Mathf.Cos(InpJoint.J1) * Mathf.Sin(InpJoint.J2 + InpJoint.J3 + InpJoint.J4); float r33 = Mathf.Cos(InpJoint.J2 + InpJoint.J3 + InpJoint.J4); return(new Vector3(r13, r33, r23)); }
public void SetSlider(Joint6 joint) { slider1.value = joint.J1; slider2.value = joint.J2; slider3.value = joint.J3; slider4.value = joint.J4; slider5.value = joint.J5; slider6.value = joint.J6; }
private void Awake() { testSfear1 = GameObject.CreatePrimitive(PrimitiveType.Sphere); testSfear1.transform.parent = SUPER.transform; testSfear2 = GameObject.CreatePrimitive(PrimitiveType.Sphere); testSfear2.transform.localScale = SUPER.transform.localScale / 2; testSfear2.transform.parent = SUPER.transform; InpJoint = new Joint6(0, -45, 90, 0, 0, 0); }
protected void GetPlot(Joint6 joint, int deltaTime) { for (int i = 0; i < vectorsArray.Length; i++) { for (int j = vectorsArray[i].Length - 1; j > 0; j--) { vectorsArray[i][j] = vectorsArray[i][j - 1]; vectorsArray[i][j].x -= deltaTime * dataToCOORD.x; } vectorsArray[i][0].Set(sizeWindow.x / 2, joint.ToArray()[i] * dataToCOORD.y - sizeWindow.y / 2); } }
protected void MessageManager(byte[] data) { try { print((char)data[data.Length - 1]); switch ((char)data[data.Length - 1]) { case 'J': //kinematic.InpJoint.IntoByteSteam(data); byte[] sortData = Unpack(data); print(sortData.Length); if (sortData.Length == 12) { Joint6 test = new Joint6(); test.IntoByteSteam(sortData); print(test.ToString()); graf.jointGraf.SetJoint6(test.J1 + 180, test.J2 + 180, test.J3 + 180, test.J4 + 180, test.J5 + 180, test.J6 + 180); //float[] s = Joint6.IntoByteSteamStatic(sortData); } else { Debug.Log("Сообщение не полное"); TerminalMethod("Сообщение не полное"); } break; case 'S': break; default: Debug.Log("Неизвестное сообщение: " + System.Text.Encoding.UTF8.GetString(data, 0, data.Length)); TerminalMethod("Неизвестное сообщение"); break; } } catch (NullReferenceException) { print("Нет сообщения"); } catch (IndexOutOfRangeException) { } }
private void ForwardKinematic() { // Inp J1, J2 ... // Out x,y,z, a,b,c float l1 = 7, l2 = 5, l3 = 6.7f, l4 = 3; Joint6 localInpJoint = inpJoint * Mathf.Deg2Rad; float xz = l2 * Mathf.Sin(localInpJoint.J2) + l3 * Mathf.Sin(localInpJoint.J2 + localInpJoint.J3) + l4 * Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4); float x = Mathf.Sin(localInpJoint.J1) * xz; float z = Mathf.Cos(localInpJoint.J1) * xz; float y = l1 + l2 * Mathf.Cos(localInpJoint.J2) + l3 * Mathf.Cos(localInpJoint.J2 + localInpJoint.J3) + l4 * Mathf.Cos(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4); Vector3 orientVector = new Vector3(Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4) * Mathf.Sin(localInpJoint.J1), Mathf.Cos(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4), Mathf.Sin(localInpJoint.J2 + localInpJoint.J3 + localInpJoint.J4) * Mathf.Cos(localInpJoint.J1)); float angleA = Mathf.Atan2(orientVector.y, Mathf.Sqrt(Mathf.Pow(orientVector.x, 2) + Mathf.Pow(orientVector.z, 2))) * Mathf.Rad2Deg; //inpVector = new Vector5(x, y, z, angleA, inpJoint.J5, inpJoint.J6); inpVector.SetVector5(x, y, z, angleA, inpJoint.J5, inpJoint.J6); }
protected IEnumerator AutoWork() { int atualSpeed; while (workMode == WorkMode.Auto) { yield return(new WaitForSeconds(1f)); atualSpeed = scenceControl.targetProperties.speed; // отправляем изменённую программу if (scenceControl.ProgramVectors != programmVectorPackage && scenceControl.ProgramVectors != null && programmWork == true) { programmVectorPackage = scenceControl.ProgramVectors; Joint6[] jointsArray = new Joint6[programmVectorPackage.Length]; byte[] byteArray = new byte[programmVectorPackage.Length * 12]; for (int i = 0; programmVectorPackage.Length > i; i++) { jointsArray[i] = Kinematic.InverseKinematicStatic(programmVectorPackage[i]); byte[] pieceByte = jointsArray[i].ToByteSteam(); pieceByte.CopyTo(byteArray, 12 * i); } protocol.SendRequest(byteArray, "P"); } // отправляем изменёнyю скорость if (speed != atualSpeed) { speed = atualSpeed; protocol.SendRequest(BitConverter.GetBytes((ushort)speed), "S"); } } }
protected IEnumerator JointMoveC(Joint6 jointVar, GameObject[] joints) { int resolution = 5; float[] smoothTime = new float[] { 1, 1, 1, 1, 1 }; GameObject[] joint = new GameObject[5]; for (int i = 0; i < joint.Length; i++) { joint[i] = joints[i]; } joints[2].transform.localRotation = Quaternion.Euler(lim3Ext.Limitation(jointVar.J2, jointVar.J3), 0, 0); for (int i = 1; i <= resolution; i++) { joints[0].transform.localRotation = Quaternion.Euler(0, Mathf.SmoothDampAngle(joint[0].transform.localRotation.eulerAngles.y, lim1.Limitation(jointVar.J1), ref smoothTime[0], i / (float)resolution), 0); joints[1].transform.localRotation = Quaternion.Euler(Mathf.SmoothDampAngle(joint[1].transform.localRotation.eulerAngles.x, lim2.Limitation(jointVar.J2), ref smoothTime[1], i / (float)resolution), 0, 0); joints[3].transform.localRotation = Quaternion.Euler(Mathf.SmoothDampAngle(joint[3].transform.localRotation.eulerAngles.x, lim4.Limitation(jointVar.J4), ref smoothTime[3], i / (float)resolution), 0, 0); joints[4].transform.localRotation = Quaternion.Euler(0, Mathf.SmoothDampAngle(joint[4].transform.localRotation.eulerAngles.y, lim5.Limitation(jointVar.J5), ref smoothTime[4], i / (float)resolution), 0); yield return(null); } }