74 lines
2.0 KiB
C#
74 lines
2.0 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
|
|
/***
|
|
*
|
|
var error = targetPosition - grabbableTransform->Position;
|
|
var command = pidState.UpdateCommand(error, f.DeltaTime, config.pidSettings, ignoreIntegration: config.ignorePidIntegrationWhileColliding && isColliding);
|
|
var impulse = FPVector3.ClampMagnitude(config.commandScale * command, config.maxCommandMagnitude);
|
|
grabbableBody->AddLinearImpulse(impulse);
|
|
grabbableTransform->Rotation = targetRotation;
|
|
*
|
|
* */
|
|
|
|
|
|
[System.Serializable]
|
|
public struct PIDSettings
|
|
{
|
|
public float proportionalGain;
|
|
public float integralGain;
|
|
public float derivativeGain;
|
|
public float maxIntegrationMagnitude;
|
|
}
|
|
|
|
[System.Serializable]
|
|
public struct PIDState
|
|
{
|
|
public PIDSettings pidSettings;
|
|
public Vector3 errorIntegration;
|
|
Vector3 lastPosition;
|
|
public Vector3 lastError;
|
|
bool derivateInitialized;
|
|
|
|
// For explanation about PID controllers, see https://www.youtube.com/watch?v=y3K6FUgrgXw
|
|
public Vector3 UpdateCommand(Vector3 error, float dt, bool ignoreIntegration = false)
|
|
{
|
|
// P
|
|
var p = pidSettings.proportionalGain * error;
|
|
|
|
// I
|
|
var i = Vector3.zero;
|
|
if (ignoreIntegration == false)
|
|
{
|
|
var newErrorIntegration = errorIntegration + dt * error;
|
|
errorIntegration = Vector3.ClampMagnitude(newErrorIntegration, pidSettings.maxIntegrationMagnitude);
|
|
i = pidSettings.integralGain * errorIntegration;
|
|
}
|
|
|
|
// D
|
|
Vector3 d = Vector3.zero;
|
|
if (derivateInitialized)
|
|
{
|
|
var errorDerivate = (error - lastError) / dt;
|
|
d = pidSettings.derivativeGain * errorDerivate;
|
|
}
|
|
|
|
// Update for next controller update
|
|
lastError = error;
|
|
derivateInitialized = true;
|
|
|
|
// Command
|
|
return p + i + d;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
Debug.LogError("Reset");
|
|
lastError = Vector3.zero;
|
|
derivateInitialized = false;
|
|
errorIntegration = Vector3.zero;
|
|
}
|
|
}
|