Lasse's code for pid controller. Needs to be redefined as a rospackage