@inka:
Du must etwas mehr Aufwand treiben, um das "Problem" zu lösen.

1. Du kannst z.B. eine Variable definieren, in der du die ZIELRICHTUNG angibst, nennen wir sie int16_t new_dir .
2. Den gemessenen Heading Wert (also die aktuelle Richtung des RP6!) tun wir in eine weitere Variable: int16_t old_dir .
3. Dann muss eine "Formel" her (nicht getestet!!!), um die Abweichung zu berechnen:
Code:
int16_t temp;
temp = new_dir - old_dir;
if (new_dir < old_dir) temp += 360;
Wenn du ein Test-Prog schreibst, würde ich das auch so machen, wie du. Als Test stellt man den RP6 in eine beliebige Richtung auf und läßt ihn in die Zielrichtung (new_dir) auf der Stelle drehen. Die Variable temp kann man dann in Rotate einsetzen. Je nach Vorzeichen (+ -) dreht man nach links oder rechts.
Wenn man das gut gemacht hat, müßte sich der RP6 immer auf kürzestem Weg zur Zielrichtung drehen.

Wahrscheinlich wird ein EINMALIGES Drehen nicht zum Ziel führen, weil Rotate ziemlich ungenau ist. In deinem Prog solltest du also noch 2-3 Korrekturschleifen einbauen.