from machine import Pin, time_pulse_us
from time import sleep, sleep_us
from servo_lib import Servo

servo = Servo(11)
TRIG = Pin(15, Pin.OUT)
ECHO = Pin(14, Pin.IN)
btnA = Pin(13, Pin.IN)
btnB = Pin(20, Pin.IN)

angle = 90
servo.set_angle(angle)
def get_distance_cm():
  TRIG.low()
  sleep_us(2)
  TRIG.high()
  sleep_us(10)
  TRIG.low()
  duration = time_pulse_us(ECHO, 1, 30000)
  if duration and duration > 0:
    return (duration * 0.0343) / 2
  return None

def change_angle(delta):
  global angle
  angle = angle + delta
  if angle < 0:
    angle = 0
  if angle > 180:
    angle = 180
  servo.set_angle(angle)
  print("Servo angle:", angle)
while True:
  dist = get_distance_cm()
  if dist is None:
    print("Distance: -- cm | Angle:", angle)
  else:
    print("Distance:", int(dist), "cm | Angle:", angle)

  if btnA.value() == 1:
    change_angle(-10)
    while btnA.value() == 1:
        sleep(0.05)
    sleep(0.15)

  if btnB.value() == 1:
    change_angle(10)
    while btnB.value() == 1:
        sleep(0.05)
    sleep(0.15)
   
  sleep(0.4)