from machine import Pin, PWM
from time import sleep

servo = PWM(Pin(15))
servo.freq(50)
button = Pin(14, Pin.IN, Pin.PULL_UP)
mode = 0

MODE_NAMES = ['SWEEP', 'HOLD', 'OFF']
btn_was_pressed = False

def angle_to_duty(angle):
        return int(1638 + angle * (8192 - 1638) / 180)

        def check_button():
                global btn_was_pressed    btn = button.value() == 0
                    if btn and not btn_was_pressed:
                                btn_was_pressed = True
                                        return True    if not btn:
                                                btn_was_pressed = False    return False
                                                print("Servo modes ready: press GP14 to cycle SWEEP -> HOLD -> OFF")

                                                while True:
                                                        if mode == 0:
                                                                    swept = False        for angle in range(0, 181, 5):
                                                                                servo.duty_u16(angle_to_duty(angle))
                                                                                            sleep(0.03)
                                                                                                        if check_button():
                                                                                                                            mode = (mode + 1) % 3
                                                                                                                                            print("[PRESS] -> " + MODE_NAMES[mode] + " | servo at " + str(angle) + " deg, dir: sweeping UP")
                                                                                                                                                            swept = True                break
                                                                                                                                                                    if not swept:
                                                                                                                                                                                    for angle in range(180, -1, -5):
                                                                                                                                                                                                        servo.duty_u16(angle_to_duty(angle))
                                                                                                                                                                                                                        sleep(0.03)
                                                                                                                                                                                                                                        if check_button():
                                                                                                                                                                                                                                                                mode = (mode + 1) % 3
                                                                                                                                                                                                                                                                                    print("[PRESS] -> " + MODE_NAMES[mode] + " | servo at " + str(angle) + " deg, dir: sweeping DOWN")
                                                                                                                                                                                                                                                                                                        break    elif mode == 1:
                                                                                                                                                                                                                                                                                                                servo.duty_u16(angle_to_duty(90))
                                                                                                                                                                                                                                                                                                                        sleep(0.02)
                                                                                                                                                                                                                                                                                                                                if check_button():
                                                                                                                                                                                                                                                                                                                                                mode = (mode + 1) % 3
                                                                                                                                                                                                                                                                                                                                                            print("[PRESS] -> " + MODE_NAMES[mode] + " | servo at 90 deg, dir: holding")
                                                                                                                                                                                                                                                                                                                                                                else:
                                                                                                                                                                                                                                                                                                                                                                            servo.duty_u16(0)
                                                                                                                                                                                                                                                                                                                                                                                    sleep(0.02)
                                                                                                                                                                                                                                                                                                                                                                                            if check_button():
                                                                                                                                                                                                                                                                                                                                                                                                            mode = (mode + 1) % 3
                                                                                                                                                                                                                                                                                                                                                                                                                        print("[PRESS] -> " + MODE_NAMES[mode] + " | servo at 0 deg, dir: off")