132 lines
4.0 KiB
C#
132 lines
4.0 KiB
C#
using UnityEngine;
|
|
|
|
public class Leg : MonoBehaviour {
|
|
public Transform Hip1;
|
|
public Transform Hip2;
|
|
public Transform Knie;
|
|
|
|
// rotation angles for different servos
|
|
public double a1 = 0;
|
|
public double a2 = 0;
|
|
public double a3 = 0;
|
|
|
|
public Vector3 point = new Vector3(10, 10, 0);
|
|
public Quaternion rotation = new Quaternion(0, 0, 0, 0);
|
|
|
|
public Vector3 transformationBeforeRotation = new Vector3(0, 0, 0);
|
|
public Vector3 transformationAfterRotation = new Vector3(0, 0, 0);
|
|
|
|
public bool invertForLeftLeg = false;
|
|
|
|
// translation to change origin to middle of body
|
|
public double translateX = 0;
|
|
public double translateY = 0;
|
|
public double translateZ = 0;
|
|
|
|
public GameObject pointShouldBe;
|
|
//public Vector3 pos = new Vector3(0, 0, 0);
|
|
|
|
public int startServo = -1;
|
|
|
|
private Foot foot = new Foot();
|
|
|
|
// Use this for initialization
|
|
void Start () {
|
|
|
|
}
|
|
|
|
// Update is called once per frame
|
|
public void FixedUpdate()
|
|
{
|
|
foot.X = point.x;
|
|
foot.Y = point.y;
|
|
foot.Z = point.z;
|
|
|
|
foot.rotationX = rotation.x;
|
|
foot.rotationY = rotation.y;
|
|
foot.rotationZ = rotation.z;
|
|
|
|
foot.translateX = translateX;
|
|
foot.translateY = translateY;
|
|
foot.translateZ = translateZ;
|
|
|
|
// transform before rotation.
|
|
foot.transformation = transformationBeforeRotation;
|
|
|
|
// transform after rotation.
|
|
foot.transformation2 = transformationAfterRotation;
|
|
|
|
foot.invert = invertForLeftLeg;
|
|
|
|
foot.setleg();
|
|
|
|
if (!invertForLeftLeg)
|
|
{
|
|
a1 = foot.O1 - 90;
|
|
a2 = 90 - foot.O2;
|
|
a3 = foot.O3 - 90;
|
|
}
|
|
else
|
|
{
|
|
a1 = (foot.O1 - 90)*-1;
|
|
a2 = 90 - foot.O2;
|
|
a3 = foot.O3 - 90;
|
|
}
|
|
|
|
if (!invertForLeftLeg && startServo != -1)
|
|
{
|
|
// Serial.Instance.send(startServo,
|
|
// (int)Remap((float)a1, -90, 90, 2500, 500),
|
|
// (int)Remap((float)a2, -90, 90, 2500, 500), // 0 is middel
|
|
// (int)Remap((float)a3, 90, -90, 2500, 500)); // 0 is middel
|
|
if(false)
|
|
Serial.Instance.send(startServo,
|
|
(int)Remap((float)a1, -90, 90, 0, 180),
|
|
(int)Remap((float)a2, 90, -90, 0, 180), // 0 is middel
|
|
(int)Remap((float)a3, -90, 90, 0, 180)); // 0 is middel
|
|
}
|
|
else if (invertForLeftLeg && startServo != -1)
|
|
{
|
|
double a1 = foot.O1 - 90;
|
|
double a2 = 90 - foot.O2;
|
|
double a3 = foot.O3 - 90;
|
|
|
|
// Serial.Instance.send(startServo,
|
|
// (int)Remap((float)a1, -90, 90, 500, 2500),
|
|
// (int)Remap((float)a2, -90, 90, 500, 2500), // 0 is middel
|
|
// (int)Remap((float)a3, 90, -90, 500, 2500)); // 0 is middel
|
|
if (false)
|
|
Serial.Instance.send(startServo,
|
|
(int)Remap((float)a1, 90, -90, 0, 180),
|
|
(int)Remap((float)a2, -90, 90, 0, 180), // 0 is middel
|
|
(int)Remap((float)a3, 90, -90, 0, 180)); // 0 is middel
|
|
}
|
|
|
|
Hip1.transform.localRotation = Quaternion.Euler(0, (float)a1, 0);
|
|
Hip2.transform.localRotation = Quaternion.Euler((float)a2, 0, 0);
|
|
Knie.transform.localRotation = Quaternion.Euler((float)a3, 0, 0);
|
|
|
|
pointShouldBe.transform.localPosition = foot.pos;
|
|
}
|
|
|
|
public Vector3 getLocalPos()
|
|
{
|
|
return point;
|
|
}
|
|
|
|
public Vector3 getWorldPos()
|
|
{
|
|
return point + new Vector3((float)translateX, (float)translateY, (float)translateZ);
|
|
}
|
|
|
|
public void setWorldPos(Vector3 pos)
|
|
{
|
|
this.point = pos - new Vector3((float)translateX, (float)translateY, (float)translateZ);
|
|
}
|
|
|
|
public static float Remap(float value, float from1, float to1, float from2, float to2)
|
|
{
|
|
return (value - from1) / (to1 - from1) * (to2 - from2) + from2;
|
|
}
|
|
}
|