implement Servo.cpp and improve Robot