76 lines
1.7 KiB
C#
76 lines
1.7 KiB
C#
|
using UnityEngine;
|
||
|
|
using UnityEngine.Serialization;
|
||
|
|
|
||
|
|
public class VectorPID : MonoBehaviour
|
||
|
|
{
|
||
|
|
[Tooltip("Just a field for user naming. Doesn't do anything.")]
|
||
|
|
[FormerlySerializedAs("name")]
|
||
|
|
public string customName;
|
||
|
|
|
||
|
|
[Tooltip("PID Constants.")]
|
||
|
|
public Vector3 PID = new Vector3(1f, 0f, 0f);
|
||
|
|
|
||
|
|
[Tooltip("The vector we are currently at.")]
|
||
|
|
[HideInInspector]
|
||
|
|
public Vector3 inputVector = Vector3.zero;
|
||
|
|
|
||
|
|
[HideInInspector]
|
||
|
|
[Tooltip("The vector we want to be at.")]
|
||
|
|
public Vector3 targetVector = Vector3.zero;
|
||
|
|
|
||
|
|
[Tooltip("Vector output from PID controller; what we read.")]
|
||
|
|
[HideInInspector]
|
||
|
|
public Vector3 outputVector = Vector3.zero;
|
||
|
|
|
||
|
|
[Tooltip("This is an euler angle, so we need to wrap correctly")]
|
||
|
|
public bool isAngle;
|
||
|
|
|
||
|
|
public float gain = 1f;
|
||
|
|
|
||
|
|
private Vector3 errorSum = Vector3.zero;
|
||
|
|
|
||
|
|
private Vector3 deltaError = Vector3.zero;
|
||
|
|
|
||
|
|
private Vector3 lastError = Vector3.zero;
|
||
|
|
|
||
|
|
private float lastTimer;
|
||
|
|
|
||
|
|
private float timer;
|
||
|
|
|
||
|
|
private void Start()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
private void FixedUpdate()
|
||
|
|
{
|
||
|
|
timer += Time.fixedDeltaTime;
|
||
|
|
}
|
||
|
|
|
||
|
|
public Vector3 UpdatePID()
|
||
|
|
{
|
||
|
|
float num = timer - lastTimer;
|
||
|
|
lastTimer = timer;
|
||
|
|
if (num != 0f)
|
||
|
|
{
|
||
|
|
Vector3 vector;
|
||
|
|
if (isAngle)
|
||
|
|
{
|
||
|
|
vector = Vector3.zero;
|
||
|
|
vector.x = Mathf.DeltaAngle(inputVector.x, targetVector.x);
|
||
|
|
vector.y = Mathf.DeltaAngle(inputVector.y, targetVector.y);
|
||
|
|
vector.z = Mathf.DeltaAngle(inputVector.z, targetVector.z);
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
vector = targetVector - inputVector;
|
||
|
|
}
|
||
|
|
errorSum += vector * num;
|
||
|
|
deltaError = (vector - lastError) / num;
|
||
|
|
lastError = vector;
|
||
|
|
outputVector = vector * PID.x + errorSum * PID.y + deltaError * PID.z;
|
||
|
|
return outputVector * gain;
|
||
|
|
}
|
||
|
|
return Vector3.zero;
|
||
|
|
}
|
||
|
|
}
|