Measure distance using echo signal from Ultrasonic sensor
raw_ultrasonic_measure measures the duration of an echo staying high. It is not related to the distance in any way, it is just a duration of trig signal.

You need a time elapsed from the beginning of a signal to reception of echo - that is, a duration of echo being low.

In a quick and dirty way,

        signal_start = micros();
        digitalWrite(Trig, HIGH);

        while (digitalRead(Echo) == 0) {
            ;
        }

        echo_received = micros();
        digital_write(Trig, LOW);

        return (echo_received - signal_start) / (2 * speed_of_sound);


In a real world application you would want to debounce echo, provide for a timeout, etc.

You may also want to account for the time taken by a first call to digitalWrite, but I doubt it will make any perceptible difference.



ultrasonic_measure is in fact a loop:

        double value = 0.0;
        for (int i = 0; i &lt; 5; i++) {
            value += raw_ultrasonic_measure(Trig, Echo);
        }
        return value / 5;


One benefit of such approach is an ability to pass a number of samples as a parameter.