added C++ code for servos