from machine import Pin, PWM, ADC, SoftI2C 
from i2c_lcd import I2cLcd # O módulo i2c_lcd.py foi importado para permitir o contato com o display lcd
from time import sleep
from mpu6050 import mpu6050, comparing_mpu6050

pinSCLGyroscopeControl = Pin(17)
pinSDAGyroscopeControl = Pin(16)

pinSCLGyroscopeReference = Pin(12)
pinSDAGyroscopeReference = Pin(14)

i2CReference = SoftI2C(scl=pinSCLGyroscopeReference, sda=pinSDAGyroscopeReference) 
i2CControl = SoftI2C(scl=pinSCLGyroscopeControl, sda=pinSDAGyroscopeControl) # Pins aos quais o acelerômetro está ligado (devem ser substituidos pelo acesso pelo MUX)


pinSDA = Pin(3) # Identifica os pins e o endereço para integração da lcd
pinSCL = Pin(23) 
enderecoLcd = 0x27

medicoesPorSegundo = 4

vel = {"X":0, "Y":0, "Z":0} # Inicialização das variáveis de ângulo

i2c = SoftI2C(scl=pinSCL, sda=pinSDA, freq=40000) # Conecta ao lcd
lcd = I2cLcd(i2c, enderecoLcd, 2, 16)


while True:
    
    sleep(0.05)
    '''
    comparedValues = comparing_mpu6050(i2CReference, i2CControl).compare_total_acceleration(medicoesPorSegundo)

    vel["X"] += comparedValues["X"]
    vel["Y"] += comparedValues["Y"]
    vel["Z"] += comparedValues["Z"]


'''
    vel["X"] += mpu6050(i2CReference).instant_acceleration()["X"]
    #vel["X"] = mpu6050(i2CReference).get_values()["AccelerometerX"]
    lcd.move_to(0,0)  # Move-se ao elemento da 1a linha e da 1a coluna do lcd
    lcd.putstr('{num}'.format(num=vel["X"])) # Escreve o valor em graus dado ao servo