Removed Set PID because both autonomous and Teleop set their own PID values
Put Change Mode before Set PID
Didn't actually change these