This is the latest balance code we've been working on since the WPI testing. The main changes:
On approach to the dock, we try to detect the location on the ramp and slow down accordingly to prevent overshoot
After the balance routine thinks the robot is level, the robot rotates 90 degrees so that the robot doesn't roll off the ramp if it's not perfectly level
This is the latest balance code we've been working on since the WPI testing. The main changes: