PID rotation controller for FRC 5112

Robot.java

// Can be called during auto periodic
private boolean rotateToDegrees(int degrees, RobotDrive myRobot, Gyro gyro) {
		double rotation = 0;
		if (Math.abs(crosstrackError) >= 0.5) {
			diffCrosstrackError = ((gyro.getAngle() - degrees) - crosstrackError) / 0.02;
			crosstrackError = (gyro.getAngle() - degrees);
			intCrosstrackError += crosstrackError * 0.02;
			rotation = -kP * crosstrackError - kD * diffCrosstrackError - kI
					* intCrosstrackError;
			if (rotation > 1)
				rotation = 1;
			else if (rotation < -1)
				rotation = -1;
			myRobot.mecanumDrive_Cartesian(0, 0, rotation, gyro.getAngle());
			return false;
		} else {
			myRobot.stopMotor();
			return true;
		}
	}

Utils.java

package org.usfirst.frc.team5112.robot;

import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Timer;

public class Utils {

	public static void rotateToDegrees(double degrees, RobotDrive myRobot,
			Gyro gyro) {
		myRobot.stopMotor();
		gyro.reset();
		double crosstrackError = (degrees - gyro.getAngle());
		double diffCrosstrackError;
		double intCrosstrackError = 0.0;
		double waitTime = 0.01;
		double rotation = 0;
		double kP = 0.2;
		double kD = 0.0000025;
		double kI = 0.001;
		while(Math.abs(crosstrackError) >= 0.5){
			 diffCrosstrackError = (gyro.getAngle() - crosstrackError)/waitTime;
			 crosstrackError = (gyro.getAngle() - degrees);
			 intCrosstrackError += crosstrackError*waitTime;
			 rotation = -kP * crosstrackError - kD * diffCrosstrackError - kI * intCrosstrackError;
			 if(rotation > 1)
				 rotation = 1;
			 else if(rotation < -1)
				 rotation = -1;
			 myRobot.mecanumDrive_Cartesian(0, 0, rotation, gyro.getAngle());
			 try {
				Thread.sleep((long) (waitTime*1000));
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}
		myRobot.stopMotor();
	}
	
	
	public static void rotateToDegreesTanh(double degrees, RobotDrive myRobot, Gyro gyro){
		myRobot.stopMotor();
		gyro.reset();
		double rotation;
		double bias;
		double error = gyro.getAngle() - degrees;
		while (!(Math.abs(error) <= 5)) {
			error = gyro.getAngle() - degrees;
			if(error < 0){
				bias = -0.2;
			} else {
				bias = 0.2;
			}
			rotation = Math.tanh(error/degrees + bias);
			myRobot.mecanumDrive_Cartesian(0, 0, rotation, gyro.getAngle());
			Timer.delay(0.01);
		}
		myRobot.stopMotor();
	}
}