Finished autonomous (& teleop)

And also fixed a crucial deg/rad bug.