Center goal servo test code

Added code to test the center goal servo with teleop controls