Help with Gyro for Field-centric Mecanum Drive

[ShadowBot]

So we have been trying to implement the gyro for field centric mecanum drive. So far, when the gyro code is hooked up to the holonomic drive, the robot spins out of control. I think I managed to fix that problem (its value used to read INF), but now the gyro angle constantly counts up at roughly 0.0001 per second and does not change when the robot rotates.

Gain is set to 0.0125 on front panel and have a button on my controller to reset the gyro.

Could our gyro be faulty or something wrong with the code?

Begin


Teleop