diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs new file mode 100644 index 000000000..76de4b9b2 --- /dev/null +++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs @@ -0,0 +1,59 @@ +using System; +using UnityEngine; + +namespace AWSIM +{ + public class FirstOrderLaggedFloat + { + private float timeConstant; + + private float desiredValue; + private float currentValue; + private float lastTime; + + public float Value + { + get + { + Update(); + return currentValue; + } + } + public float DesiredValue + { + set + { + Update(); + desiredValue = value; + } + get + { + return desiredValue; + } + } + + public FirstOrderLaggedFloat(float timeConstant, float initialValue) + { + this.timeConstant = timeConstant; + + desiredValue = currentValue = initialValue; + lastTime = Time.time; + } + + private void Update() + { + float dt = Time.time - lastTime; + + if (timeConstant == 0.0f) + { + currentValue = desiredValue; + } + else + { + currentValue += (dt / timeConstant) * (desiredValue - currentValue); + } + + lastTime = Time.time; + } + } +} \ No newline at end of file diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta new file mode 100644 index 000000000..ec3daa55e --- /dev/null +++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 963ca01e17396605cbc15c9a3f20e472 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs index d39655933..8ff3418e9 100644 --- a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs +++ b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs @@ -171,11 +171,15 @@ public float SidewaySlipMultipler // -MaxSteerAngleInput <= SteerAngleInput <= MaxSteerAngleInput. [Range(0.01f, 80)] public float MaxSteerAngleInput = 25f; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float steerAngleTimeConstant = 0.0f; // Set value to clamp AccelerationInput (m/s^2). // -MaxAccelerationInput <= AccelerationInput <= MaxAccelerationInput. [Range(0.01f, 50)] public float MaxAccelerationInput = 10; + [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")] + private float accelerationTimeConstant = 0.0f; [Header("Inputs")] @@ -189,15 +193,23 @@ public float SidewaySlipMultipler /// In the plane, output the force that will result in this acceleration. /// On a slope, it is affected by the slope resistance, so it does not match the input. /// - // TODO: Compute first order lag - public float AccelerationInput; + public float AccelerationInput + { + get => acceleration.DesiredValue; + set => acceleration.DesiredValue = Mathf.Clamp(value, -MaxAccelerationInput, MaxAccelerationInput); + } + private FirstOrderLaggedFloat acceleration; /// /// Vehicle steering input. Tire angle (degree) /// Negative is left, positive is right turn tire angle. /// - // TODO: Compute first order lag - public float SteerAngleInput; + public float SteerAngleInput + { + get => steerAngle.DesiredValue; + set => steerAngle.DesiredValue = Mathf.Clamp(value, -MaxSteerAngleInput, MaxSteerAngleInput); + } + private FirstOrderLaggedFloat steerAngle; /// /// Vehicle turn signal input. NONE, LEFT, RIGHT, HAZARD. @@ -209,6 +221,11 @@ public float SidewaySlipMultipler /// public Vector3 LocalAcceleration { get; private set; } + /// + /// Vehicle acceleration (m/s^2) + /// + public float Acceleration => acceleration.Value; + /// /// Vehicle speed (m/s) /// @@ -217,7 +234,7 @@ public float SidewaySlipMultipler /// /// Vehicle steering angle (degree) /// - public float SteerAngle => SteerAngleInput; + public float SteerAngle => steerAngle.Value; public float SteerAngleNormalized => SteerAngle / MaxSteerAngleInput; @@ -296,6 +313,12 @@ void Awake() // Initialize with non-slip value ForwardSlipMultipler = 1f; SidewaySlipMultipler = 1f; + + // Initialize acceleration + acceleration = new FirstOrderLaggedFloat(accelerationTimeConstant, 0.0f); + + // Initialize steer angle + steerAngle = new FirstOrderLaggedFloat(steerAngleTimeConstant, 0.0f); } // GroundSlipMultiplier changes the slip rate. @@ -322,10 +345,6 @@ private void OnTriggerExit(Collider other) void FixedUpdate() { - // Clamp input values. - AccelerationInput = Mathf.Clamp(AccelerationInput, -MaxAccelerationInput, MaxAccelerationInput); - SteerAngleInput = Mathf.Clamp(SteerAngleInput, -MaxSteerAngleInput, MaxSteerAngleInput); - // Compute vehicle infomation. ComputeVehicleState(); @@ -339,8 +358,7 @@ void FixedUpdate() if (sleep == false) { // Update wheel force. - var acceleration = AccelerationInput; - UpdateWheelsForce(acceleration); + UpdateWheelsForce(Acceleration); } // cache value for next frame.