Added PID controller code to autonomous mode