[Noah Kleinberg]

That sounds like a problem with the bias. You should try calling Get_Gyro_Bias() and printing the return value, which I think should be something relatively close to 512 if I remember correctly, also make sure that you called Initialize_Gyro() at some point in the code, otherwise it might be the case that the variable holding the bias was never initialized, and contains a random number, offsetting all of your data by that number.