# ==========================================================
#  Servo Position Control Using Rotary Encoder - ESP32
#  MicroPython - Wokwi Simulation
# ==========================================================

from machine import Pin, PWM
import time

# ---------------- Pin Configuration ----------------


clk = Pin(12, Pin.IN, Pin.PULL_UP)
dt  = Pin(13, Pin.IN, Pin.PULL_UP)
sw  = Pin(14, Pin.IN, Pin.PULL_UP)

# ---------------- Servo Setup ----------------
servo = PWM(Pin(17), freq=50)

# Servo angle limits
MIN_ANGLE = 0
MAX_ANGLE = 180

# Current angle (start at middle position)
angle = 90

# ---------------- Encoder State ----------------
last_clk_state = clk.value()

# ==========================================================
# Function: Convert angle (0-180) to PWM duty cycle
# Typical servo pulse: 0.5ms (0°) -> 2.5ms (180°) @ 50Hz
# ==========================================================
def set_servo_angle(angle):
    # Clamp angle within limits
    angle = max(MIN_ANGLE, min(MAX_ANGLE, angle))
    
    # Map angle (0-180) to duty (26-128) approx for 50Hz, 16-bit duty
    min_duty = 1638   # ~0.5ms pulse  (for duty_u16, 16-bit range 0-65535)
    max_duty = 8192   # ~2.5ms pulse
    
    duty = int(min_duty + (angle / 180) * (max_duty - min_duty))
    servo.duty_u16(duty)
    
    print("Servo Angle:", angle, "° | Duty:", duty)

# ==========================================================
# Encoder Interrupt Handler
# ==========================================================
def encoder_callback(pin):
    global angle, last_clk_state
    clk_state = clk.value()
    if clk_state != last_clk_state and clk_state == 0:
        dt_state = dt.value()
        if dt_state != clk_state:
            angle +=5
        else:
            angle -=5
        # clamp
        if angle > MAX_ANGLE:
            angle = MAX_ANGLE
        elif angle < MIN_ANGLE:
            angle = MIN_ANGLE
        set_servo_angle(angle)
    
    last_clk_state = clk_state

# ==========================================================
# Button Callback (Optional: Reset angle to 90°)
# ==========================================================
def button_callback(pin):
    global angle
    time.sleep_ms(50)  # debounce
    if sw.value() == 0:
        angle = 90
        set_servo_angle(angle)
        print("Reset to 90°")

# ---------------- Attach Interrupts ----------------
clk.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=encoder_callback)
sw.irq(trigger=Pin.IRQ_FALLING, handler=button_callback)

# ---------------- Initialize Servo Position ----------------
set_servo_angle(angle)

# ---------------- Main Loop ----------------
print("Rotate the encoder to control servo angle...")
while True:
    time.sleep(0.1)