Deine Kompass Lesefunktion funktioniert glaube ich nicht so wie du willst. Speziell die do/while Schleife.

Vergleiche mein I2C Lese Ablauf von Post 4 und deinen Ablauf. Du ließt den Status aus (nach dem delay(20)), verwendest aber ein NACK, danach sollte aber ein I2C-Stopp kommen. Ein NACK bedeutet das keine weiteren Daten gelesen werden, du ließt aber danach (nach 20ms) noch ein Byte.

Ich würde es eher so machen:
Code:
I2C Start (Adresse + schreiben)
I2C schreibe (0x06)
I2C rep Start (Adresse + lesen)
status = I2C Lese NACK
I2C Stopp

wenn Status OK (Daten vorhanden und keine Fehler)
{
X, Y und Z Daten auslesen
}

Was du auch noch machen kannst ist eine Hysterese einbauen, also nicht auf exakt 90° drehen, sondern wenn der Bereich zwischen z.B. 89 und 91° ist, ist er OK.

Eine weitere Funktion wäre die Geschwindigkeit der Motoren im Bereich um den Zielwinkel zu reduzieren (z.B. Winkelabweichung > 45° => PWM=100%, Winkelabweichung > 25° => PWM = 50%,...)

Beim Messen könntest du auch versuchen die Motoren abzuschalten. Also Motoren stoppen, Messwert auslesen, Ziel berechnen, Motoren wieder einschalten, delay

MfG Hannes