Updated Servo Test Code