bez HW se dá těžko odhadovat, ale typoval byc, že chyba bude tady:
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
Budeš si muset nastavit svoje „nulové“ hodnoty. Dej vozítko do roviny a nech si poslat hodnoty na seriový port. Ty pak doplň sem, nebo tam dodej funkci pro nastavení těchto hodnot při startu a stisku nějákého tlačítka.
V případě dalších problémů se budeš muset zaměřit na doladění hodnot KP, KI, KD.
Bez Hw to ale fakt asi nepujde.