Adding ability to control servo
Adding function for servo