Hi inka,
eine paar Gedanken, eine Pause in der Hauptschleife ist nicht so vorteilhaft, da Pausiert ja denn wirklich alles. Ist es denn in Deinem Projekt wirklich nötig alle 5 Servoschritte eine Messung vorzunehmen ? Wie soll das denn später funktionieren wenn der BOT sich bewegt ?
Wäre es nicht besser nur an bestimmten Positionen links, rechts und mitte die Messung durchzuführen ?
Dass der RP6 bei der blockierenden ausführung der US-messung mal z.b. kurz seine fahrt unterbrechen muss, stört mich nicht weiter...
Wieviel cm bewegt sich denn der Bot bis zur nächsten Messung ?

Wenn ich das richtig sehe, dreht Dein Servo langsam nach rechts bis er > RT erreicht hat und dann in einem Rutsch zurück nach LT. Das sieht doof aus. Warum läßt Du ihn nicht auch wieder Schrittweise nach LT drehen ? Ich habe auch eine Funktion in der Lib geschrieben wo alle meine Servos eine Grundposition einnehmen. Diese rufe ich immer vor der Hauptschleife auf (Display Zeit) und habe bei Start immer die gleiche Position.