# Importamos módulos
from machine import Pin, PWM
from hcsr04 import HCSR04
import time
#_________________________________________
servo_sg90 = PWM (Pin(21), freq = 50)
medidor1 = HCSR04(trigger_pin = 15, echo_pin = 2)
medidor2 = HCSR04(trigger_pin = 23, echo_pin = 22)
#__________________________________________
angulo= 180.0
angulo1 = 0.0
angulo2 = 90.0
while True:
distancia1 = round(medidor1.distance_cm(), 2)
print("Sensor Izquierda: ",distancia1)
distancia2 = round(medidor2.distance_cm(), 2)
print("sensor derecha: ",distancia2)
time.sleep(2)
if distancia1 > 100 and distancia2 < 3:
duty= ((3.06084 * angulo** 2 + 10278 * angulo + 550000))
conversion=duty/1000000
mili =int(conversion * 1023/20)
servo_sg90.duty(mili)
print("derecha")
time.sleep(1)
if distancia1 < 3 and distancia2 > 100:
duty= ((3.06084 * angulo1** 2 + 10278 * angulo1 + 550000))
conversion=duty/1000000
mili =int(conversion * 1023/20)
servo_sg90.duty(mili)
print("izquierda")
time.sleep(1)
if distancia1 > 100 and distancia2 > 100:
duty= ((3.06084 * angulo2** 2 + 10278 * angulo2 + 550000))
conversion=duty/1000000
mili =int(conversion * 1023/20)
servo_sg90.duty(mili)
print("izquierda")
time.sleep(1)