Thanks guys for your help.
I split Teleop into a few more subVIs and used the clean-up-diagram button, so it should be more readable now. I also took out most of the debug indicators and controls, and necessary Talon SRX get status', since we don't need them anymore. We were running the code from a build-deploy, and we got "Not running fast enough" errors a few times every second as of yesterday.
I will test again today and post logs when I get to our workshop.
If you want to look at the new code:
https://drive.google.com/file/d/0B9k...ew?usp=sharing
I added the navx libraries this time.