Merge testing back into master

Main changes are bug fixes which are critical to the running system. Side additions include adding I2C interface and Accelerometer interface. Tests have also been improved especially in `testVehicle*` department.

Rest is commented in commits and change history.