import time
from machine import Pin, PWM

# Configuración de los pines GPIO para los servos
servo1_pin = Pin(17, Pin.OUT)  # Pin GPIO 17 para el primer servomotor
servo2_pin = Pin(18, Pin.OUT)  # Pin GPIO 18 para el segundo servomotor

# Configurar PWM en los pines
servo1_pwm = PWM(servo1_pin)
servo2_pwm = PWM(servo2_pin)

# Establecer la frecuencia en 50 Hz (frecuencia estándar para servos)
servo1_pwm.freq(50)
servo2_pwm.freq(50)

# Función para mover el servomotor a un ángulo específico
def mover_servo(servo_pwm, angulo):
    # El ángulo de 0 a 180 grados se mapea a un ciclo de trabajo de 0 a 65535
    # Para un ángulo de 0 grados, el ciclo de trabajo debe ser aproximadamente 2%
    # Para un ángulo de 180 grados, el ciclo de trabajo debe ser aproximadamente 12%
    duty_cycle = int((angulo / 180) * 1000 + 2500)  # Mapeo de 0-180 grados a 2500-12500 microsegundos
    servo_pwm.duty_u16(duty_cycle)  # Ajustar el ciclo de trabajo en el rango correcto (0-65535)
    time.sleep(1)  # Esperar 1 segundo para que el servo se mueva

# Bucle principal para mover los servomotores
try:
    while True:
        # Mover servomotor 1 y 2 a 0 grados
        mover_servo(servo1_pwm, 0)
        mover_servo(servo2_pwm, 0)
        time.sleep(2)

        # Mover servomotor 1 y 2 a 90 grados
        mover_servo(servo1_pwm, 90)
        mover_servo(servo2_pwm, 90)
        time.sleep(2)

        # Mover servomotor 1 y 2 a 180 grados
        mover_servo(servo1_pwm, 180)
        mover_servo(servo2_pwm, 180)
        time.sleep(2)

except KeyboardInterrupt:
    pass