import machine
import time
from mpu6050_lib import MPU6050

# Configura el sensor MPU6050
i2c = machine.I2C(sda=machine.Pin(4), scl=machine.Pin(5))
mpu = MPU6050(i2c)

# Configura la frecuencia de muestreo
frecuencia_muestreo = 100  # Hz
intervalo_muestreo = 1 / frecuencia_muestreo

# Configura el rango de aceleración (ajusta según sea necesario)
mpu.accel_range(16)  # ±16g

# Función para calcular la aceleración RMS
def calcular_aceleracion_rms(samples):
    suma_cuadrados = sum([sample ** 2 for sample in samples])
    aceleracion_rms = (suma_cuadrados / len(samples)) ** 0.5
    return aceleracion_rms

# Inicializa una lista para almacenar las muestras de aceleración
muestras_aceleracion = []

# Bucle principal
while True:
    # Lee los valores del acelerómetro
    aceleracion = mpu.get_acceleration()
    
    # Calcula la magnitud de la aceleración
    magnitud_aceleracion = (aceleracion[0]**2 + aceleracion[1]**2 + aceleracion[2]**2) ** 0.5
    
    # Agrega la magnitud de la aceleración a la lista de muestras
    muestras_aceleracion.append(magnitud_aceleracion)
    
    # Si hemos acumulado suficientes muestras para calcular el RMS, hazlo
    if len(muestras_aceleracion) >= frecuencia_muestreo:
        aceleracion_rms = calcular_aceleracion_rms(muestras_aceleracion)
        print("Aceleración RMS:", aceleracion_rms)
        
        # Borra las muestras anteriores
        muestras_aceleracion = []
    
    # Espera el intervalo de muestreo
    time.sleep(intervalo_muestreo)