Code cleanup. Added driving information to serial output.