from machine import Pin, PWM
from time import sleep

# Servo Setup
servo = PWM(Pin(12))
servo.freq(50)

# Button Setup (Using internal Pull-Ups)
# Connect buttons between the GP pin and GND
pb1 = Pin(25, Pin.IN, Pin.PULL_UP) # Center <-> Left
pb2 = Pin(19, Pin.IN, Pin.PULL_UP) # Center <-> Right

# State Variables
pb1_state = "center" # Tracks if PB1 should go Left or Center next
pb2_state = "center" # Tracks if PB2 should go Right or Center next

def set_servo(percent):
    """Calculates pulse and sets servo position"""
    min_us = 500
    max_us = 2500
    pulse_us = min_us + (percent / 100) * (max_us - min_us)
    servo.duty_ns(int(pulse_us * 1000))

# Initialize at Center (50%)
set_servo(50)
print("System Ready - Servo at Center (50%)")

while True:
    # PB1 Logic: Tengah (50) <-> Kiri (0)
    if pb1.value() == 0:  # Button pressed
        if pb1_state == "center":
            print("PB1: Moving to Left (0%)")
            set_servo(0)
            pb1_state = "left"
        else:
            print("PB1: Moving to Center (50%)")
            set_servo(50)
            pb1_state = "center"
        
        sleep(0.3)  # Debounce delay

    # PB2 Logic: Tengah (50) <-> Kanan (100)
    if pb2.value() == 0:  # Button pressed
        if pb2_state == "center":
            print("PB2: Moving to Right (100%)")
            set_servo(100)
            pb2_state = "center_alt" # Using a diff state to keep logic clean
        else:
            print("PB2: Moving to Center (50%)")
            set_servo(50)
            pb2_state = "center"
            
        sleep(0.3)  # Debounce delay

    sleep(0.01) # Small loop delay to save CPU