void FixedUpdate()
{
Throttle = Mathf.Clamp(Throttle, 0, 1);
_pidThrottle = pid.Update((angle * Mathf.PI) / 180, gameObject.transform.rotation.z * 2, Time.fixedDeltaTime);
rightMotor.CreateForce(Throttle + _pidThrottle);
leftMotor.CreateForce(Throttle - _pidThrottle);
}
void FixedUpdate()
{
verticalSpeed = gameObject.GetComponent <Rigidbody2D>().velocity.y;
_currentAltitude = gameObject.transform.position.y;
_pidThrottle = pid.Update(altitude, _currentAltitude, Time.fixedDeltaTime);
if (altitude - _currentAltitude < -5) //Engage Descend Speed Limiter if altitude difference is greater than 5
{
_pidThrottle = DescendSpeedLimiter(_pidThrottle, verticalSpeed, descendMaxSpeed);
}
//Anti Integral Windup Start
if (verticalSpeed < -2f) //Descending
{
pid.LimitIntegral(0);
}
pid.LimitIntegral(integralLimit); // Ascending
//Anti Integral Windup End
if (verticalSpeed > ascendMaxSpeed) //Ascend Speed Limiter
{
_pidThrottle = 0;
}
rightMotor.CreateForce(_pidThrottle);
leftMotor.CreateForce(_pidThrottle);
}