Getting Gyro Working w/ Mecanum Drive

[WillNess]

Although our season is over, the programming team is still tasked with completing several things before the start of the next season. One of the more difficult things (and something I struggled with implementing) during the build season was using a gyro to help the drivers. This is the code I've come up with so far, but I only included the drive code, nothing else:

teleopPeriodic(){
   driveCode();
}
    public static void driveCode(){
    	RobotMap.calculateTurnSpeedsAndAngles();
        if(OI.stick.getZ() < .1){
        	double subtractValue = 0; 
        	if(Robot.turnSpeedForGyroCorrection > .3){
        		subtractValue = Robot.turnSpeedForGyroCorrection - .3;
        	}else{
                         //I do not want to robot traveling past 30%, so this subtractValue will never let it go past 50%
        		subtractValue = 0;
        	}
        	RobotMap.setVelocityGyro(OI.stick.getX(), OI.stick.getY(), OI.stick.getZ());
        }else{
        	RobotMap.setVelocity(OI.stick.getX(), OI.stick.getY(), OI.stick.getZ());
        	Robot.setAngle = RobotMap.driveGyro.getAngle();
        }
    }

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);
	    	drivefrontRight.set(fR);
	    	driverearLeft.set(rL);
	    	driverearRight.set(rR);
    	}else{
    		drivefrontLeft.set(fL+z/2);
    		drivefrontRight.set(fR+z/2);
    		driverearLeft.set(rL+z/2);
    		driverearRight.set(rR+z/2);
    	}
    }
    public static void setVelocityGyro(double x,double y,double z){
    	        RobotMap.calculateTurnSpeedsAndAngles();
		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;
		double errorOfAngles = Robot.actualAngle - Robot.setAngle;
		double turnSpeedRight = 0;
		double turnSpeedLeft = 0;
		if(errorOfAngles > 0){
			if(fL > 0){
				turnSpeedLeft = Math.abs(Robot.turnSpeedForGyroCorrection);
			}else{
				turnSpeedLeft = -Math.abs(Robot.turnSpeedForGyroCorrection);
			}
			turnSpeedRight = 0;
		}else if(errorOfAngles < 0){
			turnSpeedLeft = 0;
			if(fR > 0){
				turnSpeedRight = Math.abs(Robot.turnSpeedForGyroCorrection);
			}else{
				turnSpeedRight = -Math.abs(Robot.turnSpeedForGyroCorrection);
			}
		}else{
			turnSpeedLeft = 0;
			turnSpeedRight = 0;
		}
            drivefrontLeft.set(fL - turnSpeedLeft);
	    drivefrontRight.set(fR - turnSpeedRight);
	    driverearLeft.set(rL - turnSpeedLeft);
	    driverearRight.set(rR - turnSpeedRight);
    } 
    public static void calculateTurnSpeedsAndAngles(){
    	double turnValue = 0.02;
    	Robot.actualAngle = RobotMap.driveGyro.getAngle();
    	double angleError = Robot.actualAngle - Robot.setAngle;
    	Robot.turnSpeedForGyroCorrection = angleError * turnValue;
    }

Sorry for the weird spacing, that's just how the copy+paste did it. Any help would be appreciated! Also: This is untested