LoginSignup
1
1

More than 3 years have passed since last update.

@MuAuan(ムー ウワン)

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

Last updated at Posted at 2021-04-10

今回は、以下の３つのモデュールを利用した。
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

temp_real68.77.png

加速度センサーに基づきカメラを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()

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

まとめ

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

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

1
1
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
1
1