Added custom for servos, and cleaned up code