from machine import Pin, I2C , PWM
import ssd1306
from mpu6050 import MPU6050
import math

# 初始化I2C总线和OLED显示屏
# i2c = I2C(scl=Pin(22), sda=Pin(21))
oled = ssd1306.SSD1306_I2C(128, 64, i2c)

buzzer = Pin(2,Pin.OUT)
buzzer = PWM(2)
buzzer.freq(1000)

# 初始化陀螺仪
mpu = MPU6050(19,18)
m_roll = 0
m_pitch = 0
def get_angle():
    roll = 0
    pitch = 0
    gx, gy, gz = mpu.GetGyro()
    ax, ay, az = mpu.GetAccel()

    # 计算加速度计的倾斜角度
    roll_acc = math.atan2(-ay, az) * (180 / math.pi)
    pitch_acc = math.atan2(ax, math.sqrt(ay*ay + az*az)) * (180 / math.pi)

    # 计算陀螺仪的角速度
    dt = 0.01  # 采样时间间隔，根据实际情况调整
    roll_gyro = gx * dt
    pitch_gyro = gy * dt

    # 融合加速度计和陀螺仪的数据，得到最终角度
    alpha = 0.98  # 加速度计权重，根据实际情况调整
    roll = alpha * (roll_gyro + roll_acc) + (1 - alpha) * roll
    pitch = alpha * (pitch_gyro + pitch_acc) + (1 - alpha) * pitch

    return roll, pitch

while True:
    # 读取陀螺仪数据
    m_roll, m_pitch = get_angle()
    # accel_x, accel_y, accel_z = mpu.GetAccel()
    # gyro_x, gyro_y, gyro_z = mpu.GetGyro()

    # 清空OLED屏幕
    oled.fill(0)

    # 显示加速度数据
    oled.text("ANGLE:", 0, 0)
    oled.text("roll: {}".format(m_roll), 0, 10)
    oled.text("pitch: {}".format(m_pitch), 0, 20)
    
    if (abs(m_roll) > 50 or abs(m_pitch) > 50):
        oled.text("HELP ME!!!", 20, 40)
        buzzer.duty(100)
    else:
        oled.text("I'm OK!!!", 20, 40)
        buzzer.duty(0)
    # 更新OLED显示屏
    oled.show()