Update ServoObject.cpp

Created a separate method for setting the controller, and also moved all implementation from header file to this file