I reverted a lot of changes that I made earlier and Patrick added some PID controller functionality.