CSharpRoboticsLib.Drive.Interfaces.GyroscopeDriveExtensions.TurnToAngle C# (CSharp) Method

        public static void TurnToAngle(this IGyroscopeDrive d, IMotionController controller, double angle, double tolerance, bool brake, double interval = 0.02)
        {
            controller.SetPoint = angle;

            while (Math.Abs(d.Gyroscope.GetAngle() - angle) > tolerance)
            {
                double power = controller.Get(d.Gyroscope.GetAngle());
                d.SetPowers(power, -power);
                AccurateWaitSeconds(interval);
            }

            if(brake)
               d.SetPowers(0, 0);
        }