'''
深圳市普中科技有限公司（PRECHIN 普中）
技术支持：www.prechin.net
PRECHIN
 普中

实验名称：MPU6050陀螺仪加速度传感器实验
接线说明：MPU6050陀螺仪加速度传感器模块-->ESP32 IO
         SCL-->25
         SDA-->26
         VCC-->5V
         GND-->GND
         
实验现象：程序下载成功后，转动传感器，在Shell控制台输出XY轴角度值

注意事项：

'''

from machine import Pin, I2C
import utime
import math

# ------------------- 本地模式配置（无外网访问） -------------------
# 硬件接线：SCL-->25, SDA-->26, VCC-->5V, GND-->GND
# 功能：仅在Shell控制台输出MPU6050的XY轴角度值
# ----------------------------------------------------------------------

# MPU6050本地驱动（无需网络）
class MPU6050:
    def __init__(self, i2c, addr=0x68):
        self.i2c = i2c
        self.addr = addr
        # 唤醒MPU6050（默认可能睡眠）
        utime.sleep_ms(100)
        self.i2c.writeto_mem(self.addr, 0x6B, bytes([0]))
    
    def setSampleRate(self, rate):
        # 设置采样率（200Hz）
        rate_div = 1000 // rate - 1
        self.i2c.writeto_mem(self.addr, 0x19, bytes([rate_div]))
    
    def setGResolution(self, res):
        # 设置加速度计量程（±2g）
        res_map = {2:0x00, 4:0x08, 8:0x10, 16:0x18}
        if res in res_map:
            self.i2c.writeto_mem(self.addr, 0x1C, bytes([res_map[res]]))
    
    def readData(self):
        # 读取加速度计数据并转换为g值
        utime.sleep_ms(5)
        data = self.i2c.readfrom_mem(self.addr, 0x3B, 6)
        ax = (data[0] << 8) | data[1]
        ay = (data[2] << 8) | data[3]
        az = (data[4] << 8) | data[5]
        # 处理16位有符号数据（避免负数计算错误）
        if ax > 32767: ax -= 65536
        if ay > 32767: ay -= 65536
        if az > 32767: az -= 65536
        # 返回包含Gx、Gy、Gz的对象
        class AccelData:
            def __init__(self, x, y, z):
                self.Gx = round(x / 16384.0, 4)  # 2g量程：16384 LSB/g
                self.Gy = round(y / 16384.0, 4)
                self.Gz = round(z / 16384.0, 4)
        return AccelData(ax, ay, az)

# 全局变量
mpu = None
i2c = None

# 初始化硬件（I2C+MPU6050）
def setup():
    global mpu, i2c
    # 1. 初始化I2C（SCL=25, SDA=26，仿真平台兼容）
    try:
        i2c = I2C(1, scl=Pin(25), sda=Pin(26), freq=100000)
        utime.sleep_ms(200)
        print("I2C初始化完成（编号1）")
    except Exception as e:
        print(f"I2C编号1初始化失败，尝试编号0：{e}")
        i2c = I2C(0, scl=Pin(25), sda=Pin(26), freq=100000)
        utime.sleep_ms(200)
        print("I2C初始化完成（编号0）")
    
    # 2. 扫描I2C设备（确认MPU6050连接）
    devices = i2c.scan()
    print(f"I2C总线上的设备（十进制）: {devices}")
    print(f"I2C总线上的设备（十六进制）: {[hex(d) for d in devices]}")
    
    # 3. 匹配MPU6050地址（兼容0x68和0x69）
    target_addr = 0x68 if 0x68 in devices else 0x69 if 0x69 in devices else None
    if not target_addr:
        print("❌ 未找到MPU6050设备！")
        print("请检查：1. 接线是否为SCL=25、SDA=26  2. 模块是否供电正常")
        return False
    
    # 4. 初始化MPU6050并配置参数
    try:
        mpu = MPU6050(i2c, target_addr)
        mpu.setSampleRate(200)  # 采样率200Hz
        mpu.setGResolution(2)   # 量程±2g
        print(f"✅ MPU6050初始化成功（地址：0x{target_addr:X}）")
        print("📊 开始读取角度数据（Shell控制台输出）：")
        print("-" * 50)
        return True
    except OSError as e:
        print(f"❌ MPU6050初始化失败：{e}")
        return False

# 均值滤波（减少数据波动）
def averageMPU(count, timing_ms):
    global mpu
    if not mpu:
        return 0, 0, 0
    gx, gy, gz = 0, 0, 0
    gxoffset = 0.05  # 偏移校正值（可根据实际调整）
    gyoffset = -0.03
    valid_count = 0
    for _ in range(count):
        try:
            data = mpu.readData()
            gx += data.Gx - gxoffset
            gy += data.Gy - gyoffset
            gz += data.Gz
            valid_count += 1
            utime.sleep_ms(timing_ms)
        except OSError:
            utime.sleep_ms(timing_ms)
    # 避免除以零，返回平均值
    return (round(gx/valid_count, 4) if valid_count else 0,
            round(gy/valid_count, 4) if valid_count else 0,
            round(gz/valid_count, 4) if valid_count else 0)

# 主循环（实时读取+计算+显示）
def loop():
    while True:
        # 1. 读取MPU6050数据并滤波
        gx, gy, gz = averageMPU(30, 3)
        # 2. 计算向量模长（避免除以零）
        vdim = math.sqrt(gx**2 + gy**2 + gz**2)
        if vdim < 0.1:
            utime.sleep_ms(100)
            continue
        
        # 3. 计算XY轴角度（弧度转角度，保留2位小数）
        rad2deg = 180 / math.pi
        angleX = round(rad2deg * math.asin(gx / vdim), 2)
        angleY = round(rad2deg * math.asin(gy / vdim), 2)
        # 角度方向校正（与实际转动方向一致）
        angleX_final = round(-angleX, 2)
        angleY_final = round(-angleY, 2)
        
        # 4. Shell控制台清晰输出（每秒10次）
        print(f"angleY = {angleY_final:>6.2f} ° | angleX = {angleX_final:>6.2f} °")
        utime.sleep(0.1)

# 程序入口
if __name__ == '__main__':
    try:
        if setup():
            loop()
        else:
            print("❌ 程序初始化失败，退出")
    except KeyboardInterrupt:
        print("\n" + "-" * 50)
        print("📤 程序手动终止")
    except Exception as e:
        print(f"\n❌ 程序异常终止：{e}")