from machine import Pin, PWM, ADC
import time

# --- Instellingen ---
PIN_SERVO = 2
PIN_POT = 34     # GPIO 35 (Let op: gebruik ADC1 pinnen op ESP32)

# --- Initialisatie ---
# 1. Servo
servo = PWM(Pin(PIN_SERVO))
servo.freq(50)

# 2. Potmeter
pot = ADC(Pin(PIN_POT))
pot.atten(ADC.ATTN_11DB) # Bereik 0 - 3.3V

# --- Hulpfunctie: Map ---
# Zet een getal om van de ene schaal naar de andere
def map_waarde(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

print("Servo besturing gestart. Draai aan de potmeter!")

while True:
    # 1. Lees potmeter (0 tot 65535)
    pot_val = pot.read_u16()
    
    # 2. Bereken puls lengte in nanoseconden
    # Ingang: 0 tot 65535
    # Uitgang: 1.000.000 (1ms) tot 2.000.000 (2ms)
    # Wil je een groter bereik? Verander dan 1000000 naar 500000 (0.5ms) en 2000000 naar 2500000 (2.5ms)
    puls_ns = map_waarde(pot_val, 0, 65535, 500000, 2500000)
    
    # 3. Stuur naar servo
    servo.duty_ns(puls_ns)
    
    # Even wachten voor stabiliteit
    time.sleep(0.02)