Working Stuff

Most of the code is still commented, but the limit switches work, the
latch works in teleop, and all the commented stuff still theoretically
works.