Added new files for logging, servo config
Did some doxygen, started modularization