Add test code in TeleOp for Servo.h