from machine import Pin, PWM, ADC
import time

# Configuración de los pines PWM para los servos
servo1 = PWM(Pin(21), freq=50)  # Servo 1 en el pin 21
servo2 = PWM(Pin(22), freq=50)  # Servo 2 en el pin 22

# Configuración de los pines ADC para el joystick
joystick_x = ADC(Pin(34))  # Eje X del joystick
joystick_y = ADC(Pin(35))  # Eje Y del joystick

# Configuración del rango de los ADC (lectura de 0 a 4095)
joystick_x.atten(ADC.ATTN_11DB)  # Rango 0-3.3V
joystick_y.atten(ADC.ATTN_11DB)

def map_value(value, in_min, in_max, out_min, out_max):
    """
    Mapea un valor de un rango de entrada a un rango de salida.
    """
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

def set_servo_angle(pin, angle):
    """
    Ajusta el ángulo del servo.
    :param pin: Objeto PWM configurado para el servo.
    :param angle: Ángulo deseado (0 a 180 grados).
    """
    min_us = 500  # Ancho de pulso para 0 grados
    max_us = 2500  # Ancho de pulso para 180 grados
    duty_us = min_us + (angle / 180) * (max_us - min_us)
    duty_cycle = int((duty_us / 20000) * 1023)  # Convertir a ciclo de trabajo (0-1023)
    pin.duty(duty_cycle)

try:
    while True:
        # Leer valores del joystick
        x_val = joystick_x.read()  # Lectura del eje X
        y_val = joystick_y.read()  # Lectura del eje Y

        # Mapear los valores del joystick (0-4095) a ángulos (0-180)
        servo1_angle = map_value(x_val, 0, 4095, 0, 180)
        servo2_angle = map_value(y_val, 0, 4095, 0, 180)

        # Ajustar los ángulos de los servos
        set_servo_angle(servo1, servo1_angle)
        set_servo_angle(servo2, servo2_angle)

        # Pausa para estabilizar
        time.sleep(0.1)

except KeyboardInterrupt:
    # Detener el PWM al interrumpir el programa
    servo1.deinit()
    servo2.deinit()
    print("Programa detenido")