Update servo.cpp