Created a basic servo controller and added it to CMake