# Distance -> servo + LED on XIAO RP2040, ~15 readings/s.
# HC-SR04: TRIG -> D0 (GPIO26), ECHO -> divider -> D2 (GPIO28). MG90S signal -> D4 (GPIO6).
# >= 40 cm: servo 90 deg; <= 5 cm: servo 0 deg; linear in between. <= 15 cm: on-board LED red ON.
# No echo: everything holds its last state.
import machine, time

trig = machine.Pin(26, machine.Pin.OUT, value=0)    # D0
echo = machine.Pin(28, machine.Pin.IN)              # D2
led = machine.Pin(17, machine.Pin.OUT, value=1)     # on-board red, active-low (0 = on)

servo = machine.PWM(machine.Pin(6))                 # D4
servo.freq(50)
MIN_US, MAX_US = 500, 2500                          # MG90S 0..180 deg; tweak if 0/90 look off

NEAR_CM, FAR_CM = 5, 40                             # servo 0 deg at NEAR, 90 deg at FAR
LED_CM = 15


def servo_deg(deg):
    servo.duty_ns(int((MIN_US + (MAX_US - MIN_US) * deg / 180) * 1000))


def distance_cm():
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)
    t = machine.time_pulse_us(echo, 1, 30000)       # 30 ms timeout (~5 m)
    return None if t < 0 else t * 0.0343 / 2


servo_deg(90)
while True:
    cm = distance_cm()
    if cm is None:
        print("no echo")
    else:
        c = min(max(cm, NEAR_CM), FAR_CM)
        deg = round(90 * (c - NEAR_CM) / (FAR_CM - NEAR_CM))
        servo_deg(deg)
        led.value(0 if cm <= LED_CM else 1)
        print("%.1f cm | servo %d deg | LED %s" % (cm, deg, "ON" if cm <= LED_CM else "off"))
    time.sleep_ms(66)                               # ~15 Hz
