Added PI controller, various minor fixes

First working version