Changed Servo interface

I also added a servo test program.