Genau so etwas habe ich schon programmiert. Ich habe einen GP2D12 auf einem Schrittmotor montiert.
Je nach Schrittmotorstellung schreibe ich dann den Entfernungswert in ein Array. Man muss es natürlich so organisieren, dass man immer weiß, in welcher Stelle des Arrays welcher Abstandswert (Winkel) abgespeichert wurde.
Nach einer Richtungsänderung vom Schrittmotor (+-36° bei mir) werte ich dann das gesamte Array aus, d.h. ich schaue wo die geringste und wo die höchste Entfernung gemessen wurde. Dann noch ein paar Berechnungen, ob ich ausweichen muss oder nicht .. und wenn ich ausweichen muss, dann zum Wert mit dem größten Abstand.

Wenn du genauere fragen zur Realisierung hast, kannst du gerne fragen.

Viele Grüße
CsT