##############################################################
#                Servo Motor Interface                       #
##############################################################
#
# Control a Servo Motor with Raspberry Pi Pico using MicroPython (Hardware & Simulation)
#
# Check out the link for Code explanation and Hardware details
# Link: 
#   http://tech.arunkumarn.in/blogs/raspberry-pi-pico/how-to-control-a-servo-motor-with-raspberry-pi-pico-using-micropython-complete-guide/
#
#

from machine import Pin, PWM
from time import sleep

# Initialize PWM on GPIO 16
servo_pin = Pin(16)
servo = PWM(servo_pin)

# PWM Configuration for SG90 Servo
FREQUENCY = 50  # 50Hz standard for servos
MIN_DUTY = 1638  # ~0.5ms pulse → 0°
MAX_DUTY = 7864  # ~2.4ms pulse → 180°

servo.freq(FREQUENCY)

def servo_angle(angle):
    """Map angle (0-180) to duty cycle (1638-7864)"""
    angle = max(0, min(180, angle))  # Clamp to valid range
    duty = MIN_DUTY + (angle / 180) * (MAX_DUTY - MIN_DUTY)
    servo.duty_u16(int(duty))

try:
    while True:
        # Sweep: 0° → 45° → 90° → 180° → 135° → 90° → 0°
        for angle in [0, 45, 90, 135, 180,  90, 0]:
            servo_angle(angle)
            sleep(1)
            
except KeyboardInterrupt:
    print("\nStopping servo...")
    servo.deinit()  # Disable PWM output