import RPi.GPIO as GPIO
import time
GPIO.setmode( GPIO.BCM )
GPIO.setwarnings( 0 )

print( "sr04 print" )

sr04_trig = 10
sr04_echo = 11

GPIO.setup( sr04_trig, GPIO.OUT )
GPIO.setup( sr04_echo, GPIO.IN, pull_up_down=GPIO.PUD_DOWN )

def sr04( trig_pin, echo_pin ):
    """
    Return the distance in cm as measured by an SR04
    that is connected to the trig_pin and the echo_pin.
    These pins must have been configured as output and input.s
    """

    GPIO.output(trig_pin, False)  # altijd met false starten
    time.sleep(0.5)
    GPIO.output(trig_pin, True)
    time.sleep(0.00001)  # pulse trigger
    GPIO.output(trig_pin, False)

    while GPIO.input(echo_pin) == False:
        start_time = time.time()  # tijd opslaan wanneer de echo false is

    while GPIO.input(echo_pin) == True:
        end_time = time.time()  # tijd opslaan wanneer de echo true is

    total_time = end_time - start_time  # totaal aantal afstand in seconden
    return (round(total_time*34300/2, 2), total_time)  #tuple terug geven met afstand in cm en totaal aantal seconden
    # sensor is niet helemaal naukeurig(~1-2cm variatie), kan ook aan de rpi timing liggen

try:
    while True:
        print( sr04( sr04_trig, sr04_echo ))
        time.sleep( 0.5 )
except KeyboardInterrupt:
    GPIO.cleanup()