Trivial modifications

Just moved Serial.begin to top of setup so it doesn't add unnecessary
delay after motors have already started. Also changed while loop so the
PDController starts getting called after each wheel gets 2 interrupts
instead of 3.