Created a P-controller (similar to lab) that listens to param server
Fixed stupid ros::ok bug, which would make the node un-interruptable
Fixed bugs. Cleaned code. Fixed params. Still P-controller
Updated readme