I don't see anything obviously wrong. Looking at your code, I don't think it will correct for NaN errors though. By outputting your values to SmartDashboard with code like
Code:
SmartDashboard.putNumber("Raw Gyro Angle",Robot.gyro.getRawAngle())
, you can see what is going on.