# main.py ── Raspberry Pi Pico W + MPU‑6050 (OLED 表示 + キャリブレーション & フィルタ版)
# ・起動時に静置キャリブレーション（オフセット補正）
# ・IIR ローパスフィルタで揺れを低減
# ・DLPF 10 Hz・±250 °/s に設定
# ・ジャイロ角速度を OLED にリアルタイム表示
#
# 2025‑07‑03 sakura lain 用
from machine import Pin, I2C
import time, struct, ssd1306
# ───────────────────────────────
# 定数・設定
# ───────────────────────────────
I2C_ID = 0 # I2C0 (SDA=GP16, SCL=GP17)
I2C_FREQ = 400_000 # 400 kHz
MPU_ADDR = 0x68 # AD0=LOW → 0x68
GYRO_SENS = 131.0 # ±250°/s → 131 LSB/°/s
ALPHA = 0.2 # IIR フィルタ係数 (0<α≤1)
CAL_SAMPLES = 200 # キャリブレーション試行回数
CAL_DELAY_MS = 5 # キャリブレーション間隔
# ───────────────────────────────
# インスタンス生成
# ───────────────────────────────
i2c = I2C(I2C_ID, sda=Pin(16), scl=Pin(17), freq=I2C_FREQ)
oled = ssd1306.SSD1306_I2C(128, 64, i2c)
# ───────────────────────────────
# MPU‑6050 操作関数
# ───────────────────────────────
def mpu_write(reg: int, data: int) -> None:
i2c.writeto_mem(MPU_ADDR, reg, bytes([data]))
def mpu_read(reg: int, length: int) -> bytes:
return i2c.readfrom_mem(MPU_ADDR, reg, length)
def mpu_init() -> None:
"""
DLPF: 10 Hz / ±250 °/s / ±8 g で初期化
"""
mpu_write(0x6B, 0x00) # PWR_MGMT_1: スリープ解除
mpu_write(0x1B, 0x00) # GYRO_CONFIG: ±250 °/s
mpu_write(0x1C, 0x10) # ACCEL_CONFIG: ±8 g
mpu_write(0x1A, 0x05) # CONFIG: DLPF 10 Hz
time.sleep_ms(100)
def read_raw():
"""0x3B から 14 byte 読み出し → tuple(raw_ax, raw_ay, raw_az, raw_tmp, raw_gx, raw_gy, raw_gz)"""
return struct.unpack(">hhhhhhh", mpu_read(0x3B, 14))
# ───────────────────────────────
# キャリブレーション（静置でオフセット取得）
# ───────────────────────────────
def calibrate_gyro():
sum_gx = sum_gy = sum_gz = 0
for _ in range(CAL_SAMPLES):
_, _, _, _, gx, gy, gz = read_raw()
sum_gx += gx
sum_gy += gy
sum_gz += gz
time.sleep_ms(CAL_DELAY_MS)
return (sum_gx // CAL_SAMPLES,
sum_gy // CAL_SAMPLES,
sum_gz // CAL_SAMPLES)
# ───────────────────────────────
# OLED 画面レイアウト
# ───────────────────────────────
def oled_header():
oled.fill(0)
oled.text("MPU-6050 Gyro", 0, 0)
oled.text("X: dps", 0, 16)
oled.text("Y: dps", 0, 32)
oled.text("Z: dps", 0, 48)
oled.show()
def oled_update(x, y, z, old):
"""
値が変わった軸のみ部分書き換え (x,y,z: float, old: dict)
"""
def _draw(val, ypos, key):
if val != old[key]:
oled.fill_rect(24, ypos, 48, 8, 0)
oled.text("{:+6.2f}".format(val), 24, ypos)
old[key] = val
_draw(x, 16, 'x')
_draw(y, 32, 'y')
_draw(z, 48, 'z')
oled.show()
# ───────────────────────────────
# メイン
# ───────────────────────────────
def main():
print("Initializing MPU‑6050 …")
mpu_init()
print("Calibrating gyro … (keep sensor still)")
off_gx, off_gy, off_gz = calibrate_gyro()
print("Offset =", off_gx, off_gy, off_gz)
oled_header()
# IIR フィルタ初期値
filt_x = filt_y = filt_z = 0.0
prev = {'x': None, 'y': None, 'z': None}
while True:
# ── 生データ取得 & オフセット補正 ───────────────────────
_, _, _, _, raw_gx, raw_gy, raw_gz = read_raw()
gx = (raw_gx - off_gx) / GYRO_SENS # °/s
gy = (raw_gy - off_gy) / GYRO_SENS
gz = (raw_gz - off_gz) / GYRO_SENS
# ── IIR ローパスフィルタ ───────────────────────────
filt_x += ALPHA * (gx - filt_x)
filt_y += ALPHA * (gy - filt_y)
filt_z += ALPHA * (gz - filt_z)
# ── OLED 更新 ────────────────────────────────────
oled_update(filt_x, filt_y, filt_z, prev)
# ── デバッグ出力 (USB‑Serial) ─────────────────────
print("gyr_fx:{:+.2f},gyr_fy:{:+.2f},gyr_fz:{:+.2f}"
.format(filt_x, filt_y, filt_z))
time.sleep_ms(50) # 20 Hz 表示
# スクリプト直実行時のみ
if __name__ == "__main__":
main()
0 件のコメント:
コメントを投稿