Waitms 1 is ein bisschen knapp. Da is der Schall grad 30 cm in einer Richtung unterwegs. Umdrehen und zurück muss er ja auch noch.
Ich hab jetzt nicht im Kopf, wielange man warten soll, da musst du nachlesen.
Probier mal Waitms 100

Btw: den "i2cstop" zwischen leseregister festlegen und auslesen solltest du ersatzlos streichen