Bei den Winkeln ist nicht nur ein Zahlendreher, das muß auch 360 und nicht 359 sein die abgezogen oder addiert wird. Eine volle Drehung sind 360 Grad. Das wird aber auch nicht das Problem sein, sondern mehr eine kosmetische Korrektur.

Ich würde auch mal vermuten, das die Motoren immer noch etwas stören.
Als einen einfachen Test könnte man den ganzen Bot einfach mal von Hand drehen und sich dabei den Winkel vom Compass ausgeben lassen und sehen wie gut das hinkommt.