Cleaned up autonomous test code

Getting ready for the real thing :)
More climber code.

Note: climber doesn't actually work yet.
More disc shooter code.

Added some stuff... Not enough commenting. Seems to have to do with
ramping up and down.
Added the disc shooting and climbing to tele-op

Pretty much.