Ich glaube die Zeichnung ist selbsterklärend, aber noch einmal:

Der Roboterposition R wird aus den 4 Sonars berechnet. Dann wird der Winkel berechnet, in dem sich der Roboter drehen müsste, wenn er nach VORNE (Y-Achse) zeigen würde, sodass er dann in Richtung P schaut.. Wie weit er sich tatsächlich drehen muss, ist berechenbar aus Richtung Aktuell minus Richtung Soll.


Wenn noch etwas unklar ist, immer raus damit.

http://img3.abload.de/img/zeichnung5ht2.png

Vielen Dank und freundliche Grüße skg-rob