import utime
from servo import Servo

# Configuración del servo en el pin GP0
s1 = Servo(0)

# Mapeo de valores para convertir el ángulo a un valor que el servo pueda utilizar
def servo_Map(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

# Función para mover el servo a un ángulo específico
def servo_Angle(angle):
    if angle < 0:
        angle = 0
    if angle > 180:
        angle = 180
    # Convierte el ángulo en el valor adecuado para el servo y mueve el servo
    # Para servos con PWM, el rango de "duty" está entre 2000 (0º) y 8000 (180º)
    s1.goto(round(servo_Map(angle, 0, 180, 2000, 8000)))

if __name__ == '__main__':
    try:
        while True:
            print("Turn left ...")
            # Gira el servo de 0 a 180 grados en incrementos de 10 grados
            for i in range(0, 180, 10):
                servo_Angle(i)
                utime.sleep(0.05)
            
            print("Turn right ...")
            # Gira el servo de vuelta de 180 a 0 grados en decrementos de 10 grados
            for i in range(180, 0, -10):
                servo_Angle(i)
                utime.sleep(0.05)
    finally:
        # Detiene el PWM cuando el programa termina
        s1.deinit()