from machine import I2C, Pin
from math import cos, sin, radians, atan2, asin, degrees
import time

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

# Активация сенсора
i2c.writeto_mem(MPU6050_ADDR, 0x6B, bytearray([0]))

# Инициализация кватерниона (без поворота)
q = [1, 0, 0, 0]

def read_raw_gyro():
    data = i2c.readfrom_mem(MPU6050_ADDR, 0x43, 6)
    return [(data[i] << 8 | data[i+1]) for i in range(0, 6, 2)]

# Функция для преобразования сырых данных гироскопа в градусы/секунду
def get_gyro():
    raw = read_raw_gyro()
    return [raw[i] / 131.0 for i in range(3)]

# Функция для обновления кватерниона на основе угловой скорости
def update_quaternion(q, gx, gy, gz, dt):
    w, x, y, z = q
    # Преобразование угловой скорости в радианы
    gx, gy, gz = radians(gx), radians(gy), radians(gz)
    # Вычисление изменения кватерниона
    q_dot = [
        -0.5 * (x * gx + y * gy + z * gz),
        0.5 * (w * gx + y * gz - z * gy),
        0.5 * (w * gy - x * gz + z * gx),
        0.5 * (w * gz + x * gy - y * gx)
    ]
    # Обновление кватерниона
    q[0] += q_dot[0] * dt
    q[1] += q_dot[1] * dt
    q[2] += q_dot[2] * dt
    q[3] += q_dot[3] * dt
    # Нормализация кватерниона
    norm = (q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) ** 0.5
    q = [qi / norm for qi in q]
    return q

# Функция для преобразования кватерниона в углы Эйлера
def quaternion_to_euler(q):
    w, x, y, z = q
    # yaw (угол вокруг оси Z)
    yaw = atan2(2.0 * (w*z + x*y), 1.0 - 2.0 * (y*y + z*z))
    return yaw

# Основной цикл
dt = 0.01  # Интервал времени в секундах

# Цель: развернуться на 180°
target_yaw = radians(180)  # Конвертация в радианы

# Максимальное количество итераций для предотвращения бесконечного цикла
max_iterations = 1000

for i in range(max_iterations):
    # Чтение данных гироскопа
    gyro_x, gyro_y, gyro_z = get_gyro()
    
    # Обновление кватерниона на основе данных гироскопа
    q = update_quaternion(q, gyro_x, gyro_y, gyro_z, dt)
    
    # Преобразование кватерниона в углы Эйлера
    yaw = quaternion_to_euler(q)
    
    print(f"Quaternion: {q}, Yaw: {degrees(yaw):.2f} degrees")

    # Проверка достижения угла в 180°
    if abs(degrees(yaw)) >= 180:
        print("Разворот завершен!")
        break
    
    time.sleep(dt)