[WillNess]

Do you think this will work? (I only included the drive code)

public void teleopInit(){
	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
        rotationSpeedForError = RobotMap.GyroMod(Jz);
        RobotMap.driveRobotDrive41.mecanumDrive_Cartesian(Jx, Jy, Jz + rotationSpeedForError, 0);
}

public double GyroMod(double rotation){
	if (gyroMode == true) {
		double error = gyro.getAngle() - gyroHeading;
		double kP = .05;
		if (rotation == 0) {
			rotation = rotation + kP * error;
		} else {
			gyroHeading = gyro.getAngle();
		}
	}
	return rotation;
	}
}