[WillNess]

I believe I have gotten gyro code to work! It is a little tricky while strafing, but for every other use it works great, and it really does help limit the turning while strafing according to our driver. So here you go, FYI: I only included code related to the driving, don't replace whole sections with this code:

public void teleopInit() {
        RobotMap.gyro.reset();
        gyroMode = true;
}

public void teleopPeriodic() {
        Jx = OI.joystick0.getX(); //Drive Joystick X
        Jy = OI.joystick0.getY(); //Drive Joystick Y
        Jz = OI.joystick0.getZ(); //Drive Joystick Z
        //gyroHeading = 0;
        if(OI.joystick0.getTrigger()){
        	rotationSpeedForError = RobotMap.GyroMod(0);
        	RobotMap.setVelocity(Jx, 0, 0); //This is strafe mode
        }else{
        	rotationSpeedForError = RobotMap.GyroMod(Jz);
            RobotMap.setVelocity(Jx, Jy, Jz);
        }
}

public static void setVelocity(double x,double y,double z){
	double fMax = Math.sin(-0*Math.PI/4+1*Math.PI/4);
	double fR = Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;
	double rL = -Math.sin(x*Math.PI/4+y*Math.PI/4)/fMax;
	double fL = -Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;
	double rR = Math.sin(-x*Math.PI/4+y*Math.PI/4)/fMax;
  	if(Math.abs(z)<.1){
  		drivefrontLeft.set(fL- rotationSpeedForError);
		drivefrontRight.set(fR - rotationSpeedForError);
   		driverearLeft.set(rL - rotationSpeedForError);
    		driverearRight.set(rR - rotationSpeedForError);
		// The reason that all of them are subtracting the error is that on our robot, the motors on the right side are flipped upside down, so to make them go the opposite way of the left, you have to flip the signs. These signs will change from bot to bot, so be prepared to change these.
    	}else{
    		drivefrontLeft.set(fL+z/2);
    		drivefrontRight.set(fR+z/2);
    		driverearLeft.set(rL+z/2);
    		driverearRight.set(rR+z/2);
		// The reason that all of them are adding the Z is that on our robot, the motors on the right side are flipped upside down, so to make them go the opposite way of the left, you have to flip the signs. These signs will change from bot to bot, so be prepared to change these.
    		gyroHeading = gyro.getAngle();
    	}
}
public static double GyroMod(double rotation){
	if (gyroMode) {
		double error = gyro.getAngle() - gyroHeading;
		double kP = .03;
		if (Math.abs(rotation) < .1) { 
			rotation = rotation + kP * error;
		}else {
			
		}
	}
	return rotation;
}

This code is also very useful because in autonomous instead of turning your robot for X amount of seconds, you just add X amount of degrees to the gyroHeading value and it will be very accurate, or in teleop you can have various buttons to fine-adjust the angle or flip the robot around, or etc.. There's many applications to this.
One thing I might try to do in the future is make the correction only read [-180,180] because when we set our heading to 0, we could rotate around 3-4 times and instead of going to the nearest "0" it would rotate back the exact amount. The reason this could help is that if the game is in a year with a goal, you could press a button and the robot would rotate to face the goal exactly, but this wouldn't work if maybe you had spun around several times before.

Thanks to: GeeTwo, Ether, and dawonn