from machine import Pin, PWM, ADC
import utime

# Configuración de los pines de los servos (PWM) y potenciómetros (ADC)
servo1 = PWM(Pin(0))  # Servo 1 en GP0
servo2 = PWM(Pin(1))  # Servo 2 en GP1
servo3 = PWM(Pin(2))  # Servo 3 en GP2

adc1 = ADC(26)  # Potenciómetro 1 en GP26 (ADC0)
adc2 = ADC(27)  # Potenciómetro 2 en GP27 (ADC1)
adc3 = ADC(28)  # Potenciómetro 3 en GP28 (ADC2)

# Configurar la frecuencia del PWM de los servos a 50 Hz
servo1.freq(50)
servo2.freq(50)
servo3.freq(50)

while True:
    # Leer los valores de los potenciómetros (valores entre 0 y 65535)
    pot1 = adc1.read_u16()
    pot2 = adc2.read_u16()
    pot3 = adc3.read_u16()

    # Convertir los valores de los potenciómetros de 0-65535 a un rango de 0-180 (grados del servo)
    angle1 = int(pot1 * 180 / 65535)
    angle2 = int(pot2 * 180 / 65535)
    angle3 = int(pot3 * 180 / 65535)

    # Convertir el ángulo a una señal de pulso en nanosegundos (para PWM)
    # Los servos generalmente se controlan con pulsos de 1ms (0 grados) a 2ms (180 grados)
    # 1ms = 1000000ns, 2ms = 2000000ns, por lo que ajustamos proporcionalmente
    duty1 = int(1000000 + (angle1 * 1000000 / 180))  # Pulsos para servo 1
    duty2 = int(1000000 + (angle2 * 1000000 / 180))  # Pulsos para servo 2
    duty3 = int(1000000 + (angle3 * 1000000 / 180))  # Pulsos para servo 3

    # Establecer el ciclo de trabajo de los servos (en nanosegundos)
    servo1.duty_ns(duty1)
    servo2.duty_ns(duty2)
    servo3.duty_ns(duty3)

    # Mostrar los valores de los potenciómetros y los ángulos
    print(f"Pot1: {pot1}, Angle1: {angle1}°")
    print(f"Pot2: {pot2}, Angle2: {angle2}°")
    print(f"Pot3: {pot3}, Angle3: {angle3}°")

    # Retardo para dar tiempo a que los servos se muevan
    utime.sleep_ms(200)