Updated the readme some more, replaced the p controller with a crude pid controller.