This is the code I hacked together to get the "LowTide" robot to drive on a heading with a field oriented mode of operation.
My early experience with the MPU6050 was not too great but eventually I stumbled across this git repo that extracts remarkably usable data from the sensor. https://github.com/jrowberg/i2cdevlib
I'm sure I'm backlevel, but this works as-is. I may update soon to allow me to use alternate I2C I/O ports.