The Stabilization PID needs some help… but we seem to like omni drive, with the field centric it really takes all of the thinking out of driving… I’ll cough up the code tomorrow, not too bad, just some simple trig and a PID. Funny enough I did this to break in our new programmer next and give him some experience, we worked out all the drive algorithms and the trig for the gyro, Good job Correy!
off topic: I think anything to do with stats should be outlawed in public places I just posted that because in my complacency I have killed 3 routers this year from reverse polarity… ::ouch:: ::ouch:: ::ouch:: ::ouch::
On topic: If anyone uses this make sure that you have the IsEnabled() in the main while loop because if the robot is disabled the integration will loose it mind causing it to go into an uncontrollable death spin when re-enabled.
My only critique is that you did not use the teleopPeriodic() method properly. It will automatically loop every 20ms, so there is no need to include your own while loop. All of the initialization code (such as zeroing the gyro) can go into teleopInit(), which will be called once at the beginning of teleop.