Large commit with many fixes

Fixed current and speed calculations. Checking for Bluetooth messages in
buffer before blocking read. Got rid of some wait loops. Added
functionality to the data logging.