from machine import Pin, ADC, PWM
import time

# Pines del joystick
joystick_x_pin = 34  # Pin VRx del joystick
joystick_y_pin = 35  # Pin VRy del joystick
joystick_sw_pin = 25  # Pin SW del joystick (opcional)

# Pines de los servos
servo_x_pin = 15  # Servo para el eje X
servo_y_pin = 16  # Servo para el eje Y

# Configuración del joystick
joystick_x = ADC(Pin(joystick_x_pin))
joystick_y = ADC(Pin(joystick_y_pin))
joystick_x.atten(ADC.ATTN_11DB)  # Configuración para rango de 0-3.3V
joystick_y.atten(ADC.ATTN_11DB)

joystick_sw = Pin(joystick_sw_pin, Pin.IN, Pin.PULL_UP)

# Configuración de los servos
servo_x = PWM(Pin(servo_x_pin), freq=50)  # Frecuencia para servos: 50 Hz
servo_y = PWM(Pin(servo_y_pin), freq=50)

# Función para mapear valores de un rango a otro
def map_value(value, in_min, in_max, out_min, out_max):
    return int((value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Función para mover el servo a un ángulo específico
def set_servo_angle(servo, angle):
    # Convertir el ángulo (0-180) a ciclo de trabajo (500-2400 us)
    duty = map_value(angle, 0, 180, 26, 123)  # Ajustar según la resolución de PWM (0-1023)
    servo.duty(duty)

while True:
    # Leer los valores del joystick
    x_value = joystick_x.read()  # Lectura del eje X (0-4095)
    y_value = joystick_y.read()  # Lectura del eje Y (0-4095)

    # Mapear los valores del joystick (0-4095) a ángulos (0-180 grados)
    x_angle = map_value(x_value, 0, 4095, 0, 180)
    y_angle = map_value(y_value, 0, 4095, 0, 180)

    # Mover los servos a los ángulos calculados
    set_servo_angle(servo_x, x_angle)
    set_servo_angle(servo_y, y_angle)

    # Imprimir valores para depuración
    print(f"Joystick X: {x_value} -> Servo X: {x_angle}")
    print(f"Joystick Y: {y_value} -> Servo Y: {y_angle}")

    # Comprobar si el botón del joystick está presionado
    if joystick_sw.value() == 0:
        print("Botón del joystick presionado")

    # Espera para estabilizar las lecturas
    time.sleep(0.05)