Initial implementation of ServoWrite