from machine import Pin,PWM
from time import sleep
import math
from servo import Servo

servo1 = Servo(15)

last_angle = 0

def servo_position(com):
    global last_angle
    if com[0] == "servo":
        angle = int(com[1])
        if 180 >= angle >= 0:
            servo1.move(angle)
            last_angle = angle
            print("Servo moved in desired angle")
            return True
        else:
            print("This angle is invalid, please insert an angle between 0-180")
            return False

    elif com[0] == "position":
        print("The servo is at currently angle", last_angle)
        return True
    
    else:
        print("Unknown command")
        return False



print("Hello, ESP32!")

servo1.move(0)



while True:
    command = input(">")
    x = command.split()
    servo_position(x)
    sleep(0.1)