Update teleop.c

Adds belt and arm functionality