from machine import Pin, SoftI2C 
import ssd1306
from time import sleep
from hcsr04 import HCSR04 # para murcielago


# ESP32 Pin assignment 
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))

oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

oled.text("TOUCH Pin 14 ", 0, 30)
oled.text('OR 33 to STOP ', 0, 40)
#oled.text('Hello, World 3!', 0, 20)
        
oled.show()

from machine import I2C
from machine import Pin
from machine import sleep
import mpu6050
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))     #initializing the I2C method for ESP32
#i2c = I2C(scl=Pin(5), sda=Pin(4))       #initializing the I2C method for ESP8266
mpu= mpu6050.accel(i2c)

mpu.get_values()
a=mpu.get_values()
xx=a["aX"]
yy=a["aY"]
zz=a["aZ"]
gx=a["gX"]
gy=a["gY"]
gz=a["gZ"]

# murcielago ESP32
murcielago = HCSR04(trigger_pin=5, echo_pin=18, echo_timeout_us=10000)


from machine import TouchPad, Pin
import time
tp1 = TouchPad(Pin(33,Pin.IN,Pin.PULL_UP))
tp2 = TouchPad(Pin(14,Pin.IN,Pin.PULL_UP))
t=-1
while True: # (tp1.read()>200) and (tp2.read()>200):
    t = (t+1)%20  #temporizador para el murcielago (2 segundos)
    if (t==0):
        distance = murcielago.distance_cm() #murcielago cm
        print(distance)

    mpu.get_values()
    a=mpu.get_values()
    #s=str(a["aX"]-xx)+" "+str(a["aY"]-yy)+" "+str(a["aZ"]-zz)
    #g=str(a["gX"]-gx)+" "+str(a["gY"]-gy)+" "+str(a["gZ"]-gz)
    x=int((a["aY"]-yy)/50)+64
    y=int((a["aX"]-xx)/100)+32
    
    
    #oled.pixel(x,y ,1)
    print(x,y)
    if (x<-4)  :x=-4
    if (y<-4)  :y=-4
    if (x>125):x=122
    if (y>60) :y=58
    oled.fill(0)
    oled.text("o", x, y)
    #oled.text(g, 0, 10)
    oled.text(str((round(a["T"],2)-2)), 0, 0) #temperatura del 6050
    oled.text('Cm: '+str(int(distance)),0,50) # murcielago
    oled.show()
    sleep(100)
oled.fill(0)
oled.show()