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

今回は、以下の３つのモデュールを利用した。
[MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール](https://www.amazon.co.jp/gp/product/B008BOPN40/ref=ppx_yo_dt_b_asin_title_o04_s00?ie=UTF8&psc=1)
[SG90]()　リンクなし
[二軸カメラ架台]()　リンクなし

以下の参考②より、レジスタマップが以下の表のようになっていることが分かる．
【参考】
①[6軸IMU MPU6050概要](https://mechadiy.com/arduino-uno%E3%81%A7mpu6050%E3%82%92%E4%BD%BF%E3%81%86%E6%A6%82%E8%A6%81%E3%83%BB%E7%94%9F%E3%83%87%E3%83%BC%E3%82%BF%E5%8F%96%E5%BE%97/)
②[レジスタマップ](https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf)
以下の表を見ると、これは[MPU9250と同じaddr](https://qiita.com/MuAuan/items/b356bdac613508070f8e)ということが分かる。
つまり、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
```
![temp_real68.77.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/248a0623-d1a2-38ce-edec-cd6045dc65d9.png)



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

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

```py
    # レジスタをリセットする
    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で学ぶ電子工作](https://gendai.ismedia.jp/list/books/bluebacks/9784065193396)
④[pigpio on Raspberry pi](https://qiita.com/sireline/items/521e303f4e1fb72bca22)


```py
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()
```
上記の二つのコードを実行すると、カメラが追いかけてくれるのがわかります。
![accl_angle120.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/233744/a5d07bc2-3716-fca9-13dd-83944cc75eee.png)


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

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