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);
}
}
