Raspberry Pi Zero Wで加速度センサーの情報を読み出す(MPU6050)

ハードウェア接続構成

図1：CAN HATを接続した状態でのピン配[1]

import smbus
import math
from time import sleep
import time
import sys

DEV_ADDR = 0x68
CONFIG = 0x1A
GYRO_CONFIG = 0x1B
ACCEL_CONFIG = 0x1C

ACCEL_XOUT = 0x3b
ACCEL_YOUT = 0x3d
ACCEL_ZOUT = 0x3f
TEMP_OUT = 0x41
GYRO_XOUT = 0x43
GYRO_YOUT = 0x45
GYRO_ZOUT = 0x47

PWR_MGMT_1 = 0x6b
PWR_MGMT_2 = 0x6c   

bus = smbus.SMBus(1)

#デバイスの初期化処理
#----------------------------------------------------------------------------------
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x80)#write_byte_data(int addr,char cmd,char val) 1:I2C通信対象のアドレス。今回は加速度センサのみなので68固定,2:レジスタのアドレス,3:値)0x80はデバイスリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_1, 0x00)
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x07)#FIFO/I2C_MST/SIG_CONDのリセット
bus.write_byte_data(DEV_ADDR, PWR_MGMT_2, 0x00)
#----------------------------------------------------------------------------------

#センサーの設定
#----------------------------------------------------------------------------------
bus.write_byte_data(DEV_ADDR, CONFIG, 0x00)#CONFIG
bus.write_byte_data(DEV_ADDR, GYRO_CONFIG, 0x00)#250°/s
bus.write_byte_data(DEV_ADDR, ACCEL_CONFIG, 0x00)#2g
#----------------------------------------------------------------------------------


def read_word(adr):#センサーの値が2バイトであらわされるため、2バイト分のデータを結合する関数
    high = bus.read_byte_data(DEV_ADDR, adr)#上位バイトのデータ
    low = bus.read_byte_data(DEV_ADDR, adr+1)#下位バイトのデータ
    val = (high << 8) + low#上位バイトのデータをビット1バイト分(8bit)左にシフトして、下位バイトのデータと足し合わせて1つのデータにしている。
    return val

def read_word_sensor(adr):
    val = read_word(adr)
    if (val >= 0x8000):  return -((65535 - val) + 1)#0x8000が最上位ビットが立っている状態なので、マイナス。もし、マイナス値だった場合は符号を反転させてる。
    else:  return val

def get_temp():
    temp = read_word_sensor(TEMP_OUT)
    x = temp / 340 + 36.53      # data sheet(register map)記載の計算式.
    return x

def getGyro():
    x = read_word_sensor(GYRO_XOUT)/ 131.0# data sheet(register map)記載の計算式.(250°を選択した場合)
    y = read_word_sensor(GYRO_YOUT)/ 131.0
    z = read_word_sensor(GYRO_ZOUT)/ 131.0
    return [x, y, z]


def getAccel(): 
    x = read_word_sensor(ACCEL_XOUT)/ 16384.0# data sheet(register map)記載の計算式.(2gを選択した場合)
    y= read_word_sensor(ACCEL_YOUT)/ 16384.0
    z= read_word_sensor(ACCEL_ZOUT)/ 16384.0
    return [x, y, z]


while True:
    try:
        ax, ay, az = getAccel()#加速度センサーの情報読み出し
        gx, gy, gz = getGyro()#ジャイロセンサーの情報読み出し
        magnitude_a = math.sqrt(ax**2+ay**2+az**2)#加速度の大きさを算出
        temp = get_temp()#温度センサーの情報読み出し
        print ('GYRO:{:4.3f},{:4.3f},{:4.3f},' .format(gx, gy, gz)) 
        print ('ACC:{:4.3f},{:4.3f},{:4.3f},' .format(ax, ay, az))    
        print ('ACC_MAG:{:4.3f}'.format(magnitude_a))
        print ('TEMP:{:4.3f}'.format(temp))
    except:
        bus.close()
        print("Error Occur")
        sys.exit()

まとめ