Ich kann schon fortgeschrittenes C++, aber ich hab hier nicht die nötige HW Bild  
So eine Art Radar war auch mein gedanke. 4 US Sensoren, je 90° Winkel. Dann dieses "Radar" einfach immer 90° hin- und herbewegen (Servo etc) und für jeden Punkt schauen, ob US anspricht oder nicht.