# CircuitPython Blink Example

import time
import board
import digitalio
import adafruit_hcsr04
import pwmio
from adafruit_motor import servo

sonar = adafruit_hcsr04.HCSR04(trigger_pin=board.GP5, echo_pin=board.GP6)

pwm = pwmio.PWMOut(board.GP3, duty_cycle=2 ** 15, frequency=50)
my_servo = servo.Servo(pwm)

while True:
  try:
    distance = sonar.distance
    angle = distance * 3
    print(angle)
    my_servo.angle = angle

  except RuntimeError:
    print("U bent staat te ver")
  time.sleep(2)