Pi Pico で６軸センサーのMPU-6050を使ってみる
Pi Pico で６軸センサーのMPU-6050を使ってみる

Pi Pico で６軸センサーのMPU-6050を使ってみる

MPU-6050  は加速度（３軸）・角速度（３軸）や内部温度も計測できるセンサーです。

コーディングにはMicroPython を使ってみます。ファームウェアをPico にセットしておきます。

Pico にPicroPython の環境をセット

 

元ネタ Interfacing MPU6050 with Raspberry Pi Pico & MicroPython

 

開発環境はThonny IDE （インストール済み）、母艦はラズパイ4 Model B を使います。

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

Wiring（配線）

 

 

 

 

6軸データを取得

 

 

 

 

 

 

 

 

 

 

 

Thonny を起動して、以下の２つのコードをPico側に書き込みます。

 

 

 

 

 

 

 

 

 

 

 

ライブラリーコード

【mpu6050.py】

from machine import Pin, I2C import utime PWR_MGMT_1 = 0x6B SMPLRT_DIV = 0x19 CONFIG = 0x1A GYRO_CONFIG = 0x1B ACCEL_CONFIG = 0x1C TEMP_OUT_H = 0x41 ACCEL_XOUT_H = 0x3B GYRO_XOUT_H = 0x43 def init_mpu6050(i2c, address=0x68): i2c.writeto_mem(address, PWR_MGMT_1, b'\x00') utime.sleep_ms(100) i2c.writeto_mem(address, SMPLRT_DIV, b'\x07') i2c.writeto_mem(address, CONFIG, b'\x00') i2c.writeto_mem(address, GYRO_CONFIG, b'\x00') i2c.writeto_mem(address, ACCEL_CONFIG, b'\x00') def read_raw_data(i2c, addr, address=0x68): high = i2c.readfrom_mem(address, addr, 1)[0] low = i2c.readfrom_mem(address, addr + 1, 1)[0] value = high << 8 | low if value > 32768: value = value - 65536 return value def get_mpu6050_data(i2c): temp = read_raw_data(i2c, TEMP_OUT_H) / 340.0 + 36.53 accel_x = read_raw_data(i2c, ACCEL_XOUT_H) / 16384.0 accel_y = read_raw_data(i2c, ACCEL_XOUT_H + 2) / 16384.0 accel_z = read_raw_data(i2c, ACCEL_XOUT_H + 4) / 16384.0 gyro_x = read_raw_data(i2c, GYRO_XOUT_H) / 131.0 gyro_y = read_raw_data(i2c, GYRO_XOUT_H + 2) / 131.0 gyro_z = read_raw_data(i2c, GYRO_XOUT_H + 4) / 131.0 return { 'temp': temp, 'accel': { 'x': accel_x, 'y': accel_y, 'z': accel_z, }, 'gyro': { 'x': gyro_x, 'y': gyro_y, 'z': gyro_z, } } 1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950 from machine import Pin, I2Cimport utime PWR_MGMT_1 = 0x6BSMPLRT_DIV = 0x19CONFIG = 0x1AGYRO_CONFIG = 0x1BACCEL_CONFIG = 0x1CTEMP_OUT_H = 0x41ACCEL_XOUT_H = 0x3BGYRO_XOUT_H = 0x43 def init_mpu6050(i2c, address=0x68):    i2c.writeto_mem(address, PWR_MGMT_1, b'\x00')    utime.sleep_ms(100)    i2c.writeto_mem(address, SMPLRT_DIV, b'\x07')    i2c.writeto_mem(address, CONFIG, b'\x00')    i2c.writeto_mem(address, GYRO_CONFIG, b'\x00')    i2c.writeto_mem(address, ACCEL_CONFIG, b'\x00') def read_raw_data(i2c, addr, address=0x68):    high = i2c.readfrom_mem(address, addr, 1)[0]    low = i2c.readfrom_mem(address, addr + 1, 1)[0]    value = high << 8 | low    if value > 32768:        value = value - 65536    return value def get_mpu6050_data(i2c):    temp = read_raw_data(i2c, TEMP_OUT_H) / 340.0 + 36.53    accel_x = read_raw_data(i2c, ACCEL_XOUT_H) / 16384.0    accel_y = read_raw_data(i2c, ACCEL_XOUT_H + 2) / 16384.0    accel_z = read_raw_data(i2c, ACCEL_XOUT_H + 4) / 16384.0    gyro_x = read_raw_data(i2c, GYRO_XOUT_H) / 131.0    gyro_y = read_raw_data(i2c, GYRO_XOUT_H + 2) / 131.0    gyro_z = read_raw_data(i2c, GYRO_XOUT_H + 4) / 131.0     return {        'temp': temp,        'accel': {            'x': accel_x,            'y': accel_y,            'z': accel_z,        },        'gyro': {            'x': gyro_x,            'y': gyro_y,            'z': gyro_z,        }    }

 

実行コード

【main.py】

from machine import Pin, I2C import utime from mpu6050 import init_mpu6050, get_mpu6050_data i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000) init_mpu6050(i2c) while True: data = get_mpu6050_data(i2c) print("Temperature: {:.2f} °C".format(data['temp'])) print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z'])) print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'],data['gyro']['y'], data['gyro']['z'])) utime.sleep(0.5) 12345678910111213 from machine import Pin, I2Cimport utimefrom mpu6050 import init_mpu6050, get_mpu6050_data i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000)init_mpu6050(i2c) while True:    data = get_mpu6050_data(i2c)    print("Temperature: {:.2f} °C".format(data['temp']))    print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z']))    print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'],data['gyro']['y'], data['gyro']['z']))    utime.sleep(0.5)

 

 

 

 

 

 

 

 

 

 

 

 

 

傾斜角  Pitch, Roll, Yaw を計測

 

 

 

 

 

 

 

 

 

 

以下のコードは、加速度計のデータを使用して傾斜角を計算します。 ただし、ピッチ、ロール、ヨーの計算には通常、カルマン フィルターや相補フィルターの使用など、より複雑なアルゴリズムとセンサー フュージョンが必要であることに注意することが重要です。 ジャイロスコープ データは絶対角度ではなく角速度を提供するため、これらの角度を計算するにはジャイロスコープ データだけでは不十分であり、角度を取得するためにジャイロスコープ データを統合すると、時間の経過とともに大きなドリフトが発生する可能性があります。

ピッチ、ロール、ヨーの値を計算するには、加速度計とジャイロスコープのデータを組み合わせる補完フィルターを使用して、計算された角度の精度を向上させることができます。 以下は、補完的なフィルター実装を含む更新された「main.py」コードです。

from machine import Pin, I2C import utime import math from mpu6050 import init_mpu6050, get_mpu6050_data def calculate_tilt_angles(accel_data): x, y, z = accel_data['x'], accel_data['y'], accel_data['z'] tilt_x = math.atan2(y, math.sqrt(x * x + z * z)) * 180 / math.pi tilt_y = math.atan2(-x, math.sqrt(y * y + z * z)) * 180 / math.pi tilt_z = math.atan2(z, math.sqrt(x * x + y * y)) * 180 / math.pi return tilt_x, tilt_y, tilt_z def complementary_filter(pitch, roll, gyro_data, dt, alpha=0.98): pitch += gyro_data['x'] * dt roll -= gyro_data['y'] * dt pitch = alpha * pitch + (1 - alpha) * math.atan2(gyro_data['y'], math.sqrt(gyro_data['x'] * gyro_data['x'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi roll = alpha * roll + (1 - alpha) * math.atan2(-gyro_data['x'], math.sqrt(gyro_data['y'] * gyro_data['y'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi return pitch, roll i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000) init_mpu6050(i2c) pitch = 0 roll = 0 prev_time = utime.ticks_ms() while True: data = get_mpu6050_data(i2c) curr_time = utime.ticks_ms() dt = (curr_time - prev_time) / 1000 tilt_x, tilt_y, tilt_z = calculate_tilt_angles(data['accel']) pitch, roll = complementary_filter(pitch, roll, data['gyro'], dt) prev_time = curr_time print("Temperature: {:.2f} °C".format(data['temp'])) print("Tilt angles: X: {:.2f}, Y: {:.2f}, Z: {:.2f} degrees".format(tilt_x, tilt_y, tilt_z)) print("Pitch: {:.2f}, Roll: {:.2f} degrees".format(pitch, roll)) print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z'])) print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'], data['gyro']['y'], data['gyro']['z'])) utime.sleep(1) 12345678910111213141516171819202122232425262728293031323334353637383940414243444546 from machine import Pin, I2Cimport utimeimport mathfrom mpu6050 import init_mpu6050, get_mpu6050_data def calculate_tilt_angles(accel_data):    x, y, z = accel_data['x'], accel_data['y'], accel_data['z']     tilt_x = math.atan2(y, math.sqrt(x * x + z * z)) * 180 / math.pi    tilt_y = math.atan2(-x, math.sqrt(y * y + z * z)) * 180 / math.pi    tilt_z = math.atan2(z, math.sqrt(x * x + y * y)) * 180 / math.pi     return tilt_x, tilt_y, tilt_z def complementary_filter(pitch, roll, gyro_data, dt, alpha=0.98):    pitch += gyro_data['x'] * dt    roll -= gyro_data['y'] * dt     pitch = alpha * pitch + (1 - alpha) * math.atan2(gyro_data['y'], math.sqrt(gyro_data['x'] * gyro_data['x'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi    roll = alpha * roll + (1 - alpha) * math.atan2(-gyro_data['x'], math.sqrt(gyro_data['y'] * gyro_data['y'] + gyro_data['z'] * gyro_data['z'])) * 180 / math.pi     return pitch, roll i2c = I2C(0, scl=Pin(21), sda=Pin(20), freq=400000)init_mpu6050(i2c) pitch = 0roll = 0prev_time = utime.ticks_ms() while True:    data = get_mpu6050_data(i2c)    curr_time = utime.ticks_ms()    dt = (curr_time - prev_time) / 1000     tilt_x, tilt_y, tilt_z = calculate_tilt_angles(data['accel'])    pitch, roll = complementary_filter(pitch, roll, data['gyro'], dt)     prev_time = curr_time     print("Temperature: {:.2f} °C".format(data['temp']))    print("Tilt angles: X: {:.2f}, Y: {:.2f}, Z: {:.2f} degrees".format(tilt_x, tilt_y, tilt_z))    print("Pitch: {:.2f}, Roll: {:.2f} degrees".format(pitch, roll))    print("Acceleration: X: {:.2f}, Y: {:.2f}, Z: {:.2f} g".format(data['accel']['x'], data['accel']['y'], data['accel']['z']))    print("Gyroscope: X: {:.2f}, Y: {:.2f}, Z: {:.2f} °/s".format(data['gyro']['x'], data['gyro']['y'], data['gyro']['z']))    utime.sleep(1)

Thonny Shell コンソールでは、加速度計、ジャイロスコープ、温度値、および X、Y、Z 軸の傾斜角を確認できます。 これとは別に、ピッチとロールの値も表示されます。 しかし、ヨー値はまだ存在しないままです。

上記のコードは相補フィルターを使用してピッチ角とロール角を計算することに注意してください。 ただし、ヨー角の計算には方位情報が含まれるため、磁力計が必要です。これは、加速度計とジャイロスコープだけでは正確に導き出すことができません。

MPU-6050 には磁力計が内蔵されていないため、MPU-6050 だけを使用してヨーを正確に計算することはできません。 ヨーを計算したい場合は、HMC5883L などの追加センサーを使用するか、MPU-9250 や MPU-9255 などの磁力計が内蔵された IMU を使用する必要があります。

 

 

 

 

 

 

 

 

 

 

 

 

 

Appendix

Arduino IDE 版はこちら

マイコンボードで６軸センサーのMPU-6050を使ってみる

 

 

📎📎📎📎📎📎📎📎📎📎
BOT