controlling speed when adding torque to rotate rigidbody

I have a rigid body which I am applying torque to in order to rotate it toward many other objects. This look left at object A, then look over to the right at object B.

I am using a modified version of the scripts from this question, I updated to restrict rotation to 1 axis.

Scripts are working except for one issue.

The rotation is spot on, but the Time it takes to rotation to complete seems to be proportional to the distance away the object is. Ex. if the object is 100 units away, rotating 45 degrees takes 1 second, but if the object is 1 unit away, rotating the same angular distance takes 10+ seconds!

Is there something simple I’m missing to control the rotation speed?


public class PlayerControllerScript : MonoBehaviour {

	private readonly VectorPid angularVelocityController = new VectorPid(33.7766f, 0, 0.2553191f);
	private readonly VectorPid headingController = new VectorPid(9.244681f, 0, 0.06382979f);

	public void RotateTo(Transform pDestTransform)
	{

		var angularVelocityError = rigidbody.angularVelocity * -1;
		//Debug.DrawRay(transform.position, rigidbody.angularVelocity * 10, Color.black);
		
		var angularVelocityCorrection = angularVelocityController.Update(angularVelocityError, Time.deltaTime);
		//Debug.DrawRay(transform.position, angularVelocityCorrection, Color.green);
		
		rigidbody.AddTorque(angularVelocityCorrection);
		
		var desiredHeading = pDestTransform.position - transform.position;
		//Debug.DrawRay(transform.position, desiredHeading, Color.magenta);
		
		var currentHeading = transform.forward;
		//Debug.DrawRay(transform.position, currentHeading * 15, Color.blue);
		
		var headingError = Vector3.Cross(currentHeading, desiredHeading);
		Vector3 headingCorrection = headingController.Update(headingError, Time.deltaTime);

		// we only want to rotate around y axis
		headingCorrection.x = 0;
		headingCorrection.z = 0;
		
		rigidbody.AddTorque(headingCorrection);
        }

	public void FixedUpdate ()
	{

		if (mainScript.playerMoveMode == MainScript.PlayerMoveMode.LookTowardObject1)
			RotateTo(mainScript.lookatObject1.transform);
		else if (mainScript.playerMoveMode == MainScript.PlayerMoveMode.LookTowardObject2)
			RotateTo(mainScript.lookatObject2.transform);

	}
}

public class VectorPid
{
	public float pFactor, iFactor, dFactor;
	
	private Vector3 integral;
	private Vector3 lastError;
	
	public VectorPid(float pFactor, float iFactor, float dFactor)
	{
		this.pFactor = pFactor;
		this.iFactor = iFactor;
		this.dFactor = dFactor;
	}

}	
	public Vector3 Update(Vector3 currentError, float timeFrame)
	{
		integral += currentError * timeFrame;
		var deriv = (currentError - lastError) / timeFrame;
		lastError = currentError;
		return currentError * pFactor
			+ integral * iFactor
				+ deriv * dFactor;
	}
}

This is the nature of torque. Torque is equal to force times distance. So to move objects at the same angular velocity you need to multiply the number you are using in addtorque by the distance from the pivot point.