Issue18

Implementation and tests for the PID controller. Review, comment, and if ok, we can merge it to master, and close the issue.