C++ based PID adjustment