/// <summary>
/// Prioritizes up vector.
/// </summary>
public void FaceVectors2(Vector3D forward, Vector3D up)
{
// In (pitch, yaw, roll)
Vector3D error = -GetAngles2(rc.WorldMatrix, forward, up);
Vector3D angles = new Vector3D(anglePID.Control(error));
ApplyGyroOverride(rc.WorldMatrix, angles);
}
public Vector3D ControlPosition(Vector3D targetPosition, Vector3D targetVelocity, double maxSpeed = double.PositiveInfinity)
{
return(ControlVelocity(targetVelocity + VectorClamp(-pid.Control(rc.GetPosition() - targetPosition), maxSpeed)));
}
