Added code to control servos