Add PID controller, it's totally functional now, however, the code
structure needs to be improved.