[DominickC]

I believe you'll find that once you get to competition, bridge balancing is going to be quite easier to be handled manually by the drivers to compensate for inconsistencies like the number of bots on the bridge and so on. Since the bridges are calibrated with springs to return to 0 degrees, they are pretty easy to balance on.

If you still want to try a form of software balancing, more power to you. I did mine similar to how you did yours the first time around. The way you wrote your code the first time was pretty good, but just needs a few changes.

Since the bridges are considered balanced if they are within 15 degrees either way of 0, you don't need to be directly at 0 degrees, nor would the Gyro be accurate enough to bring you to 0 degrees exactly. I'd say < 3.5 and > 3.5 to give a 7 degree drift.

I'm not sure how your bot's drive train works, but 0.75 is quite speedy for ours. You might consider telling your drivers to drive up onto the ramp fully THEN engage the auto-balancing code, which may allow you to use slower motor outputs, leading to a more accurate balance.

As someone else suggested, you should nestle your case structures. If the < 3.5 case structure is false, check to see if it's > 3.5. If that's false too, set the motor outputs to 0.

Dom