private void GoToPosUpdate(object sender, PIDParam param)
{
var error = param.target - rb.position;
var targetVel = PIDUpdate(error, param.maxVal, _posPID, ref lastError, ref totalError);
var force = (targetVel - rb.velocity) / Time.fixedDeltaTime;
var gravity = rb.useGravity ? (Physics.gravity * -1) : new Vector3(0, 0, 0);
rb.AddForce(force + gravity, ForceMode.Acceleration);
}
private void TurnToRotUpdate(object sender, PIDParam param)
{
var error = param.target.y - rb.transform.rotation.eulerAngles.y;
if (Mathf.Abs(error) > 360 - Mathf.Abs(error))
{
if (error > 0)
{
error = Mathf.Abs(error) - 360;
}
else
{
error = 360 - Mathf.Abs(error);
}
}
var vel = PIDUpdate(new Vector3(0, error, 0), param.maxVal, _rotPID, ref lastError, ref totalError);
rb.angularVelocity = vel;
}
public void TurnToRot(float targetRotY, float maxSpeed)
{
ResetError();
_pidParam = new PIDParam(new Vector3(0, targetRotY, 0), maxSpeed);
_pidHandler = TurnToRotUpdate;
}
public void GoToPos(Vector3 targetPos, float maxSpeed)
{
ResetError();
_pidParam = new PIDParam(targetPos, maxSpeed);
_pidHandler = GoToPosUpdate;
}
