【センサー入門】RasPi4でMPU-6050加速度、温度、角速度センサーとカメラ自動追跡で遊んでみた♪

今回は、以下の３つのモデュールを利用した。
MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール
SG90　リンクなし
二軸カメラ架台　リンクなし

以下の参考②より、レジスタマップが以下の表のようになっていることが分かる．
【参考】
①6軸IMU MPU6050概要
②レジスタマップ
以下の表を見ると、これはMPU9250と同じaddrということが分かる。
つまり、MPU9250と同じプログラムで取得できるということである。

Addr(Hex)	Addr(Dec.)	Register Name	R/W	Bit7 - Bit0
3B	59	ACCEL_XOUT_H	R	ACCEL_XOUT[15:8]
3C	60	ACCEL_XOUT_L	R	ACCEL_XOUT[7:0]
3D	61	ACCEL_YOUT_H	R	ACCEL_YOUT[15:8]
3E	62	ACCEL_YOUT_L	R	ACCEL_YOUT[7:0]
3F	63	ACCEL_ZOUT_H	R	ACCEL_ZOUT[15:8]
40	64	ACCEL_ZOUT_L	R	ACCEL_ZOUT[7:0]
41	65	TEMP_OUT_H	R	TEMP_OUT[15:8]
42	66	TEMP_OUT_L	R	TEMP_OUT[7:0]
43	67	GYRO_XOUT_H	R	GYRO_XOUT[15:8]
44	68	GYRO_XOUT_L	R	GYRO_XOUT[7:0]
45	69	GYRO_YOUT_H	R	GYRO_YOUT[15:8]
46	70	GYRO_YOUT_L	R	GYRO_YOUT[7:0]
47	71	GYRO_ZOUT_H	R	GYRO_ZOUT[15:8]
48	72	GYRO_ZOUT_L	R	GYRO_ZOUT[7:0]

###・温度計測
温度計は、こちらのシートではより明確に記載があり、以下のパラメータで計算できるとのことである。
p30

Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
Parameters:
TEMP_OUT 16-bit signed value

とのことで、以下のコードでそれらしい温度が得られた。

get_temp.py

import smbus
import time
import matplotlib.pyplot as plt

#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15) : 
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata

if __name__ == "__main__":
    address = 0x68
    channel = 1
    bus     = smbus.SMBus(channel)

    # レジスタをリセットする
    bus.write_i2c_block_data(address, 0x6B, [0x80])
    time.sleep(0.1)     

    # PWR_MGMT_1をクリア
    bus.write_i2c_block_data(address, 0x6B, [0x00])
    time.sleep(0.1)

    sk0 = time.time()
    sk = 0
    # 生データを取得する
    try:
        while True:
            data    = bus.read_i2c_block_data(address, 0x41 ,2)
            raw    = data[0] << 8 | data[1]    # 上位ビットが先
            temp    = u2s(raw)
            # Temperature in degrees C = (TEMP_OUT Register Value as a signed quantity)/340 + 36.53
            Temp_real= u2s(raw)/340 + 36.53
            print( "{0:.2f} ,Temp_real {1:.7f}".format(sk,Temp_real))
            plt.plot(sk,Temp_real, 'go')
            plt.pause(0.5)
            sk = time.time() - sk0
            #time.sleep(0.1)
    except KeyboardInterrupt:
        pass
    finally:
        plt.savefig('temp_real{0:.2f}.png'.format(sk))

途中で温めてみて、その後はずした。

$ python3 get_temp.py
0.00 ,Temp_real 27.7300000
1.06 ,Temp_real 27.7300000
1.70 ,Temp_real 27.7300000
2.32 ,Temp_real 27.7300000
...
66.03 ,Temp_real 29.4711765
66.68 ,Temp_real 29.4241176
67.38 ,Temp_real 29.3770588
68.07 ,Temp_real 29.2358824
68.77 ,Temp_real 29.3770588

###加速度センサーに基づきカメラをServoで動かす
加速度センサーに基づき常に加速度が同一になるようにカメラをServoで動かすことを考える。

####加速度センサーの測定は前回のMPU9250と同様にできる。
以下では、初期化係数rawX0などを求めている。
※ー１は、重力加速度1gという意味である

    # レジスタをリセットする
    bus.write_i2c_block_data(address, 0x6B, [0x80])
    time.sleep(0.1)     

    # PWR_MGMT_1をクリア
    bus.write_i2c_block_data(address, 0x6B, [0x00])
    time.sleep(0.1)

    data    = bus.read_i2c_block_data(address, 0x3B ,6)
    rawX0    = (2.0 / 0x8000) * u2s(data[0] << 8 | data[1])
    rawY0    = (2.0 / 0x8000) * u2s(data[2] << 8 | data[3])
    rawZ0    = (2.0 / 0x8000) * u2s(data[4] << 8 | data[5]) - 1

####Servoの制御
これは、以下の書籍から変更して使わせていただいた。
pigpioを利用している。
インストールと起動は参考③でもいいが、参考④のインストールから自動起動が分かりやすい。
【参考】
③最新 Raspberry Piで学ぶ電子工作
④pigpio on Raspberry pi

pi = pigpio.pi()
pi.set_mode(18, pigpio.OUTPUT)
pi.set_mode(19, pigpio.OUTPUT)
    try:
        while 1:
            #acc_angle_xなどを計算
            #dutyの係数はちょうどカメラの向きが一致するように決める
            duty = int(acc_angle_y*100*650/180)+ 35000
            duty1 = int((0-acc_angle_x)*100*650/180)+ 35000
            pi.hardware_PWM(18, 50, duty)
            pi.hardware_PWM(19, 50, duty1)

###全体のコード
ということで、カメラ付きの架台の方向を加速度センサーによって読み取り、サーボで自動的に制御するコードは以下のようになります。
加速度から傾きを求めるのは、誤差が大きくなるところはすっぱり０としました。

accel_camera.py

# -*- coding: utf-8 -*-

import smbus
import time
import matplotlib.pyplot as plt
import numpy as np
import pigpio

def servo_duty_hwpwm(val):
    val_min = 0
    val_max = 4095
    servo_min = 35000   # 最小デューティ比3.5%を10000倍して格納
    servo_max = 100000  # 最大デューティ比10%を10000倍して格納
    duty = int((servo_min-servo_max)*(val-val_min)/(val_max-val_min) + servo_max)

address = 0x68
channel = 1
bus     = smbus.SMBus(channel)
pi = pigpio.pi()
pi.set_mode(18, pigpio.OUTPUT)
pi.set_mode(19, pigpio.OUTPUT)

#unsignedを、signedに変換(16ビット限定）
def u2s(unsigneddata):
    if unsigneddata & (0x01 << 15) : 
        return -1 * ((unsigneddata ^ 0xffff) + 1)
    return unsigneddata

if __name__ == "__main__":
    # レジスタをリセットする
    bus.write_i2c_block_data(address, 0x6B, [0x80])
    time.sleep(0.1)     

    # PWR_MGMT_1をクリア
    bus.write_i2c_block_data(address, 0x6B, [0x00])
    time.sleep(0.1)

    # 加速度センサのレンジを±8gにする
    #bus.write_i2c_block_data(address, 0x1C, [0x08])

    data    = bus.read_i2c_block_data(address, 0x3B ,6)
    rawX0    = (2.0 / 0x8000) * u2s(data[0] << 8 | data[1])
    rawY0    = (2.0 / 0x8000) * u2s(data[2] << 8 | data[3])
    rawZ0    = (2.0 / 0x8000) * u2s(data[4] << 8 | data[5]) - 1
    sk = 0
    print ('|i|acc_X,| acc_Y,| acc_Z: |')
    print ('|:--:|:--:|:--:|:--:|')
    print ('|{0:d}|{1:.7f},|{2:.7f},|{3:.7f},|'.format(sk,rawX0, rawY0, rawZ0))
    plt.plot(sk,rawX0, 'ro', label = 'accl_angleX')
    plt.plot(sk,rawY0, 'bo', label = 'accl_angleY')
    plt.plot(sk,rawZ0, 'go', label = 'accl_rawZ')
    plt.legend()
    # データを取得する
    try:
        while 1:
            data    = bus.read_i2c_block_data(address, 0x3B ,6)
            rawX    = ((2.0 / 0x8000) * u2s(data[0] << 8 | data[1]) - rawX0)
            rawY    = ((2.0 / 0x8000) * u2s(data[2] << 8 | data[3]) - rawY0)
            rawZ    = ((2.0 / 0x8000) * u2s(data[4] << 8 | data[5]) - rawZ0)
            if np.abs(rawZ) > 0.7:
                acc_angle_x = np.degrees(np.arctan2(rawX, rawZ))
                acc_angle_y = np.degrees(np.arctan2(rawY, rawZ))
                acc_angle_z = 0
            elif np.abs(rawX) > 0.7:
                acc_angle_x = np.degrees(np.arctan2(rawX, rawZ)) 
                acc_angle_y = 0
                acc_angle_z = np.degrees(np.arctan2(rawX, rawY))
            elif np.abs(rawY) >0.7:
                acc_angle_x = 0
                acc_angle_y = np.degrees(np.arctan2(rawY, rawZ))
                acc_angle_z = np.degrees(np.arctan2(rawX, rawY))
            else:
                acc_angle_x = np.degrees(np.arctan2(rawX, rawZ))
                acc_angle_y = np.degrees(np.arctan2(rawY, rawZ))
                acc_angle_z = np.degrees(np.arctan2(rawX, rawY))
            print ('|{0:d}|{1:.7f},|{2:.7f},|{3:.7f},|'.format(sk,rawX, rawY, rawZ))
            print ('|{0:d}|{1:.7f},|{2:.7f},|{3:.7f},|'.format(sk,acc_angle_x, acc_angle_y, acc_angle_z))
            print('')
            duty = int(acc_angle_y*100*650/180)+ 35000
            duty1 = int((0-acc_angle_x)*100*650/180)+ 35000
            pi.hardware_PWM(18, 50, duty)
            pi.hardware_PWM(19, 50, duty1)
            plt.plot(sk,acc_angle_x, 'ro')
            plt.plot(sk,acc_angle_y, 'bo')
            plt.plot(sk,acc_angle_z, 'go')
            plt.pause(1)
            sk += 1
    except KeyboardInterrupt:
        pass
    finally:
        plt.savefig('accl_angle{}.png'.format(sk))

一方、カメラの読み取りは通常の以下のコードを使いました。

get_capture.py

import cv2

# VideoCapture オブジェクトを取得します
capture = cv2.VideoCapture(0)

while(True):
    ret, frame = capture.read()
    windowsize = (800, 600)
    frame = cv2.resize(frame, windowsize)

    cv2.imshow('title',frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

capture.release()
cv2.destroyAllWindows()

上記の二つのコードを実行すると、カメラが追いかけてくれるのがわかります。

###まとめ
・カメラ自動追跡を作ってみた
・MPU６０５０でもMPU9250と同様に加速度、gyro、そして温度測定ができた

・物体検出技術を利用して、カメラ自動追跡を特定の物体追跡に作り替えようと思う
・Picoに載せ替えようと思う