FINAL CODE!!!

Teleop working, auton not so much, but otherwise, it's good!