Fixed servo control. Now all servos can be controlled. Remove servo_home function