Migrated Autonomous from .h to .c

Removed autonomous testing handle from testing program
updated sensors
messing with teleop to make it work again