Add generic pid controller

See pid.cpp for examples.
To use in ros classes, add include_directories("../etc/") in CMakeLists.txt of respective package.