Added the beginning of a pid controller.
Its not pretty, but it should work....