MPU6050をRaspberry Piで使ってみた。（その１）

MPU6050という3軸ジャイロスコープ・3軸加速度センサーモジュールを搭載したチップを入手しましたので、今日はこれを使って何かやってみたいと思います。

この商品、Amazonで210円で購入しました。とてもお安いですね。

それでこれを使って何かやろうとWebから情報を漁ってみたのですが、やはりArduinoに接続して利用するケースが多いようです。私はArduinoを持っていないので（買えばいいんですけど）、Raspberry Pi（以降ラズパイ）に直接接続して使える情報を探してみたところ面白そうなサイトを見つけました。

3D OpenGL visualization of the data from an MPU-6050 connected to a Raspberry Pi

MPU6050をラズパイにつないで、OpenGLで直方体をpygameのウィンドウ内でグリグリ動かすというものです。

では早速ラズパイにMPU6050を接続します。ジャンパーワイヤーで以下のようにラズパイのGPIOピンに結線すればOKです。（ピンのレイアウトはここなどを参照ください。）

VCC => 1番ピン（3.3V電源）
GND => 6番ピン（グラウンド）
SCL => 5番ピン（GPIO3）
SDA => 3番ピン（GPIO2）

（写真ではVCCとGNDは上記とは別のピンを使用しています。）

ラズパイとの通信にはI2Cを利用するので、ラズパイのメニューから「Raspberry Piの設定」を開き、インターフェイス タブでI2Cを有効にしてください。

それで、最初に「面白そうな」と言っていたサイトでは直方体（いわゆる板）を動かしていました。そしてそのページの最後には、「センサのノイズのせいで静止してても板がブルブル震えちゃうので、この震えを抑える方法を次回の記事で紹介するね」的なことが書いてあります。まず、板というのはあまり面白くないですし、OpenGLにはサンプルのモデルとしてTeapotが実装されており簡単に呼び出せるのでこれを使うことにします。そして震えの抑制についてはその次回の記事の内容をそのまま使って、最初から震えを抑えたスクリプトを書いて動かすことにします。

このサイトのサンプルスクリプトでは、python-webpyというライブラリを利用して、簡易なWebサーバを立ててx軸とy軸の回転をクライアントに送信する仕組みをとっています。ですので、以下のようにインストールしてください。ついでにその他必要になるものもインストールします。（必要に応じてsudo apt-get updateしてから行ってください。）

sudo apt-get install python-webpy python-smbus python-opengl python-pygame

以上で準備ができたので、以下のスクリプトを実行してまずサーバ側を起動します。（私の方で震えを抑えるコードを移植しています。）

#!/usr/bin/python
import web
import smbus
import math
import time

urls = (
    '/', 'index'
)

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

gyro_scale = 131.0
accel_scale = 16384.0

address = 0x68       # This is the address value read via the i2cdetect command

def read_all():
    raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
    raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)

    gyro_scaled_x = twos_compliment((raw_gyro_data[0] << 8) + raw_gyro_data[1]) / gyro_scale
    gyro_scaled_y = twos_compliment((raw_gyro_data[2] << 8) + raw_gyro_data[3]) / gyro_scale
    gyro_scaled_z = twos_compliment((raw_gyro_data[4] << 8) + raw_gyro_data[5]) / gyro_scale

    accel_scaled_x = twos_compliment((raw_accel_data[0] << 8) + raw_accel_data[1]) / accel_scale
    accel_scaled_y = twos_compliment((raw_accel_data[2] << 8) + raw_accel_data[3]) / accel_scale
    accel_scaled_z = twos_compliment((raw_accel_data[4] << 8) + raw_accel_data[5]) / accel_scale

    return (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z)

def twos_compliment(val):
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a, b):
    return math.sqrt((a * a) + (b * b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)
    
def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

def get_z_rotation(x,y,z):
    radians = math.atan2(z, dist(x,y))
    return math.degrees(radians)

bus = smbus.SMBus(1) # or bus = smbus.SMBus(1) for Revision 2 boards

class index:
    def GET(self):

        now = time.time()

        K = 0.98
        K1 = 1 - K

        time_diff = 0.01

        (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()

        last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
        last_z = get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

        gyro_offset_x = gyro_scaled_x
        gyro_offset_y = gyro_scaled_y
        gyro_offset_z = gyro_scaled_z

        gyro_total_x = (last_x) - gyro_offset_x
        gyro_total_y = (last_y) - gyro_offset_y
        gyro_total_z = (last_z) - gyro_offset_z

        for i in range(0, int(0.5 / time_diff)):
            time.sleep(time_diff - 0.005)

            (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()

            gyro_scaled_x -= gyro_offset_x
            gyro_scaled_y -= gyro_offset_y
            gyro_scaled_z -= gyro_offset_z

            gyro_x_delta = (gyro_scaled_x * time_diff)
            gyro_y_delta = (gyro_scaled_y * time_diff)
            gyro_z_delta = (gyro_scaled_z * time_diff)

            gyro_total_x += gyro_x_delta
            gyro_total_y += gyro_y_delta
            gyro_total_z += gyro_z_delta

            rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
            rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
            rotation_z = get_z_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

            last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
            last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
            last_z = K * (last_z + gyro_z_delta) + (K1 * rotation_z)

        rotation = str(rotation_x)+" "+str(rotation_y)+" "+str(rotation_z)
        return rotation

if __name__ == "__main__":

    # Now wake the 6050 up as it starts in sleep mode
    bus.write_byte_data(address, power_mgmt_1, 0)

    app = web.application(urls, globals())
    app.run()

ターミナルで”http://0.0.0.0:8080/”と表示されればOKです。次に以下のクライアント側のスクリプトを別のターミナルから起動します。（私の方で、板の代わりにTeapotを表示するように手を加えています。）

#!/usr/bin/python

import pygame
import urllib
from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *
from math import radians
from pygame.locals import *
import sys

SCREEN_SIZE = (800, 600)
SCALAR = .5
SCALAR2 = 0.2

def resize(width, height):
    glViewport(0, 0, width, height)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(45.0, float(width) / height, 0.001, 10.0)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    gluLookAt(0.0, 1.0, -5.0,
              0.0, 0.0, 0.0,
              0.0, 1.0, 0.0)
    
def init():
    glEnable(GL_DEPTH_TEST)
    glClearColor(0.0, 0.0, 0.0, 0.0)
    glShadeModel(GL_SMOOTH)
    glEnable(GL_BLEND)
    glEnable(GL_POLYGON_SMOOTH)
    glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
    glEnable(GL_COLOR_MATERIAL)
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));

def read_values():
    link = "http://127.0.0.1:8080" # Change this address to your settings
    f = urllib.urlopen(link)
    myfile = f.read()
    return myfile.split(" ")

def run():
    pygame.init()
    screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
    resize(*SCREEN_SIZE)
    init()
    clock = pygame.time.Clock()
    angle = 0
    glutInit(sys.argv)
    
    while True:
        then = pygame.time.get_ticks()
        for event in pygame.event.get():
            if event.type == QUIT:
                return
            if event.type == KEYUP and event.key == K_ESCAPE:
                return

        values = read_values()
        x_angle = values[0]
        y_angle = values[1]

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

        glColor((1.,1.,1.))
        glLineWidth(1)
        glBegin(GL_LINES)
        
        glEnd()
        glPushMatrix()
        glRotate(float(x_angle), 1, 0, 0)
        glRotate(-float(y_angle), 0, 0, 1)

        #glutWireTeapot(1)
        glutSolidTeapot(1)
        
        glPopMatrix()
        pygame.display.flip()

if __name__ == "__main__":
    run()

pygameのウィンドウが起動して、白いTeapotが表示されれば成功です。なお上記の78行目の”glutSolidTeapot(1)”を”glutWireTeapot(1)”に変えるとワイヤーフレームのTeapotが表示されるようになります。

Raspbian JessieではGPUによるOpenGLのハードウェア・アクセラレーションが可能です。Teapotの動きがカクカクしている場合は利用してみてください。ハードウェア・アクセラレーションを有効にする方法は、ターミナルで”sudo raspi-config”と打ち、Advanced OptionsのGL Driverを選択しenabledにして再起動すればOKです。（これで”/boot/config.txt”内に”dtoverlay=vc4-kms-v3d”の一行が追加されます。）

最後に例によってYouTubeに動画をアップしたので見てみてください。

今日のところはこの辺で。☕️