Added Pid controller class, have to document it though