Servo update

Updated the code to work with the new servo design