from machine import Pin, PWM
import time

# Define the servo pin (change as needed)
SERVO_PIN = 19  # GPIO14 (D5 on ESP8266) / Change to 17 for ESP32

# Create PWM object
servo = PWM(Pin(SERVO_PIN), freq=50)  # 50Hz PWM

current_angle = None

def set_servo_angle(angle):
    global current_angle
    """Convert angle (0-180) to duty cycle and set servo position."""
    min_duty = 40  # Minimum duty cycle (~0°)
    max_duty = 115  # Maximum duty cycle (~180°)
    
    duty = int(min_duty + (angle / 180) * (max_duty - min_duty))
    servo.duty(duty)
    current_angle = angle
    print(f"Servo moved to {angle}° (Duty: {duty})")

def get_direction(angle):
    """Return the direction based on the angle."""
    if angle == 0:
        return "North"
    elif angle == 90:
        return "East"
    elif angle == 180:
        return "South"
    elif 0 < angle < 90:
        return "North East"
    elif 90 < angle < 180:
        return "South East"
    else:
        return "Unknown Direction"

def servo_current_position():
    if current_angle is not None:
        direction = get_direction(current_angle)
        print(f"Current servo position: {current_angle}° , {direction}")
    else:
        print("Servo position unknown (not set yet).")


# Terminal control loop
try:
    while True:
        angle = input("Enter angle (0-180) or 'exit' to quit or 'pos' : ")
        if angle.lower() == "exit":
            break
        elif angle.lower() == "pos":
            servo_current_position()
        elif angle.isdigit():
            angle = int(angle)
            if 0 <= angle <= 180:
                set_servo_angle(angle)
            else:
                print("Invalid range! Enter a value between 0 and 180.")
        else:
            print("Invalid input! Please enter a valid choice")
except KeyboardInterrupt:
    print("\nExiting...")

# Cleanup
servo.duty(0)
servo.deinit()
print("Servo stopped.")