Added P controller for drive distance autonomous. Needs testing.