from machine import I2C, Pin
import time
import math
import random

# Инициализация I2C
i2c = I2C(0, scl=Pin(22), sda=Pin(21), freq=400000)
MPU_ADDR = 0x68

# Настройка MPU6050
i2c.writeto_mem(MPU_ADDR, 0x6B, bytearray([0]))  # PWR_MGMT_1
i2c.writeto_mem(MPU_ADDR, 0x1C, bytearray([0]))  # ACCEL_CONFIG (±2g)

def read_accel():
    data = i2c.readfrom_mem(MPU_ADDR, 0x3B, 6)
    ax = (data[0] << 8 | data[1]) / 16384.0  # в g
    ay = (data[2] << 8 | data[3]) / 16384.0
    az = (data[4] << 8 | data[5]) / 16384.0
    return ax, ay, az

def dead_reckoning():
    # Инициализация переменных
    velocity = 0.0
    position = 0.0
    last_time = time.ticks_ms()
    
    print("time(ms),position(m),velocity(m/s),accel(m/s²)")
    
    while position < 1.0:  # Пока не пройдем 1 метр
        # Чтение акселерометра
        ax, ay, az = read_accel()
        
        # Добавляем шум ±0.1 м/с²
        noise = random.uniform(-0.1, 0.1)
        acceleration = (az - 1.0) * 9.81 + noise  # Убираем 1g и переводим в м/с²
        
        # Рассчитываем delta time
        current_time = time.ticks_ms()
        dt = time.ticks_diff(current_time, last_time) / 1000.0
        last_time = current_time
        
        # Двойное интегрирование
        velocity += acceleration * dt
        position += velocity * dt
        
        # Вывод данных для графика
        print(f"{current_time},{position:.3f},{velocity:.3f},{acceleration:.3f}")
        time.sleep(0.1)

# Калибровка (робот должен быть неподвижен)
print("Калибровка... Не двигайте робота!")
time.sleep(2)
accel_bias = [0, 0, 0]
for _ in range(100):
    ax, ay, az = read_accel()
    accel_bias[0] += ax
    accel_bias[1] += ay
    accel_bias[2] += az
    time.sleep(0.01)
accel_bias = [x/100 for x in accel_bias]
print(f"Калибровочные смещения: {accel_bias}")

# Запуск dead reckoning
print("Начинаем движение...")
dead_reckoning()
print("Робот прошел 1 метр!")