#baseado no do dqsoft
#//https://dqsoft.blogspot.com/2021/06/sensor-ultrassonico-hc-sr04-com-o.html
import utime
import time
import rp2 
from rp2 import PIO, asm_pio
from machine import Pin

# Programa para o PIO
@asm_pio(set_init=rp2.PIO.OUT_LOW,autopush=False)
def ULTRA_PIO():
    # colocar o tempo blank so na primeira que dispara
    # aguarda uma solicitação do programa
    pull()
    label('reinicio')
    mov (x,osr)
    mov (y,osr)
    
    # gera um pulso de 10 us (20 ciclos)
    set(pins,1) [1] #19
    set(pins,0)
    
    # aguarda (indefinidamente) o inicio do pulso de eco
    wait(1,pin,0)
    
    # aguarda o fim do pulso de eco
    # decrementa X a cada 2 ciclos (1us)
    label('espera')
    jmp(pin,'continue')
    jmp('Blank')        #jmp('fim')    
    label('continue')
    y_dec
    jmp(x_dec,'espera')
    label('Blank')
    jmp(not_y, 'fim')  
    jmp(y_dec, 'Blank')

    # retorna a duração do pulso de eco
    label('fim')
    mov(isr,x)
    push()
    irq(rel(0))
    jmp('reinicio')    

@asm_pio(set_init=rp2.PIO.OUT_LOW,autopush=False)    
def ULTRA_PIO2():
    # aguarda uma solicitação do programa
    pull()
    label('reinicio')
    mov (x,osr)
    # aguarda (indefinidamente) o inicio do pulso de eco
    wait(1,pin,0)
    
    # aguarda o fim do pulso de eco
    # decrementa X a cada 2 ciclos (1us)
    label('espera')
    jmp(pin,'continue')
    jmp('fim')        #jmp('fim')    
    label('continue')
    jmp(x_dec,'espera')
    
    # retorna a duração do pulso de eco
    label('fim')
    mov(isr,x)
    push()
    jmp('reinicio')   
#
@asm_pio(set_init=rp2.PIO.OUT_LOW,autopush=False)    
def ULTRA_PIO3():
   # aguarda uma solicitação do programa
    pull()
    label('reinicio')
    mov (x,osr)
    # aguarda (indefinidamente) o inicio do pulso de eco
    wait(1,pin,0)
    
    # aguarda o fim do pulso de eco
    # decrementa X a cada 1 centimetro
    label('espera')
    jmp(pin,'continue')
    jmp('fim')   
    label('continue')
    jmp(x_dec,'espera')
    
    # retorna a duração do pulso de eco
    label('fim')
    mov(isr,x)
    push()
    jmp('reinicio')   
#

def capturou(sm):
    # Print a (wrapping) timestamp, and the state machine object.
    print(time.ticks_ms(), sm)
    global bandeira
    bandeira = True

global bandeira
bandeira = True
trigger = Pin(3, Pin.OUT)
echo = Pin(2, Pin.IN)
echo2 = Pin(4, Pin.IN)
echo3 = Pin(5, Pin.IN)
sm = rp2.StateMachine(0)
sm2 = rp2.StateMachine(1)
sm3 = rp2.StateMachine(4)
sm.init(ULTRA_PIO,freq=34000,set_base=trigger,in_base=echo,jmp_pin=echo) #2000000
sm2.init(ULTRA_PIO2,freq=34000,in_base=echo2,jmp_pin=echo2) #frequancia por centimetro
sm3.init(ULTRA_PIO3,freq=34000,in_base=echo3,jmp_pin=echo3) #frequancia por centimetro
#---------------------------------------------
sm.irq(capturou)
#---------------------------------------------
sm.active(1)
sm2.active(1)
sm3.active(1)
sm.put(500)
sm2.put(500)
sm3.put(500)
while True:
  global bandeira
  if bandeira :
    val = sm.get()
    val2 = sm2.get()
    val3 = sm3.get()
    tempo = 500 - val
    tempo2 = 500 - val2
    distancia3 = 500 - val3
    distancia = tempo
    distancia2 = tempo2
    print('Frente = {0:.1f} cm'.format(distancia) )
    print('Esquerda = {0:.1f} cm'.format(distancia2) )
    print('Direita = {0:.1f} cm'.format(distancia3) )
    print('*************************')
    #//utime.sleep_ms(100)
    global bandeira
    bandeira = False