import machine, time
from machine import Pin
from HC_SR04 import HCSR04


# Create a regular p23 GPIO object
p23 = machine.Pin(23, machine.Pin.OUT)
p22 = machine.Pin(22, machine.Pin.OUT)
p21 = machine.Pin(21, machine.Pin.OUT)
p19 = machine.Pin(19, machine.Pin.OUT)


# Create another object named pwm by
# attaching the pwm driver to the pin
servo1 = machine.PWM(p23)
servo2 = machine.PWM(p22)
servo3 = machine.PWM(p21)
servo4 = machine.PWM(p19)



# Set the pulse every 20ms
servo1.freq(50)
servo2.freq(50)
servo3.freq(50)
servo4.freq(50)

# Set initial duty to 0
# to turn off the pulse
servo1.duty(0)
servo2.duty(0)
servo3.duty(0)
servo4.duty(0)

# Creates a function for mapping the 0 to 180 degrees
# to 20 to 120 pwm duty values
def map(x, in_min, in_max, out_min, out_max):
    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)

# Creates another function for turning 
# the servo according to input angle
def servo(pin, angle):
    pin.duty(map(angle, 0, 180, 20, 120))


while True:

    # To rotate the servo motor to 0 degrees
    servo(servo1, 0)
    servo(servo2, 0)
    servo(servo3, 0)
    servo(servo4, 0)

    sensor = HCSR04(trigger_pin= 18, echo_pin= 5,echo_timeout_us=1000000)

    distance = sensor.distance_cm()
    print(distance)

    serv = input("Enter Servo Number: ")

    print(f"rotating servo{serv}")

    serv = int(serv)

    if serv == 1:
    
        for i in range(90, -1, -10):
            servo(servo1, i)
            time.sleep(0.1)

        servo(servo1, 99)
        time.sleep(0.1)



    if serv == 2:
    
        for i in range(90, -1, -10):
            servo(servo2, i)
            time.sleep(0.1)

        servo(servo2, 99)
        time.sleep(0.1)

    if serv == 3:
    
        for i in range(90, -1, -10):
            servo(servo3, i)
            time.sleep(0.1)

        servo(servo3, 99)
        time.sleep(0.1)

    if serv == 4:
    
        for i in range(90, -1, -10):
            servo(servo4, i)
            time.sleep(0.1)

        servo(servo4, 99)
        time.sleep(0.1)

    else:
        print("okay")