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

from machine import Pin, PWM, I2C
import time
import struct

# ---------------- 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)
led = Pin(35, Pin.OUT)
buzzer = PWM(Pin(34, Pin.OUT), freq=50)

i2c = I2C(0, scl=Pin(22), sda=Pin(21))

# MPU6050 I2C address
MPU_ADDR = 0x68

# Wake up the MPU6050 by clearing the sleep mode bit
i2c.writeto_mem(MPU_ADDR, 0x6B, b'\x00')

# ---------------- 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()

def read_accel():
    data = i2c.readfrom_mem(MPU_ADDR, 0x3B, 6)

    ax, ay, az = struct.unpack(">hhh", data)

    return ax, ay, az
# ==========================================================
# 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()
    
    # Detect valid transition (falling edge on CLK)
    if clk_state != last_clk_state and clk_state == 0:
        dt_state = dt.value()
        
        if dt_state != clk_state:
            # Clockwise rotation -> increase angle
            angle += 5
        else:
            # Counter-clockwise rotation -> decrease angle
            angle -= 5
        
        # Clamp the angle
        angle = max(MIN_ANGLE, min(MAX_ANGLE, 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)