Fixed stupid ros::ok bug, which would make the node un-interruptable
Fixed bugs. Cleaned code. Fixed params. Still P-controller