Modified mpu6000* files in wrong location, not sure if I modified from
latest version of the files.
Finished test program, not tested on the device yet.