星期二, 2月 14, 2017

Raspberry Pi 筆記(二十六):MPU-6050 加速度計與陀螺儀感測器

[2017/02/14]
目前正流行的空拍機,主要是機上安裝了加速度計與陀螺儀感測器,使得四軸的飛行器能夠漂浮在空中。MPU-6050是一個價格不高的六軸感測器,模組載有加速度計與陀螺儀的功能,我們可以透過 I2C 讀取感測器的值,作為改變飛行的方向與速度。以下就來看看如何利用Raspberry Pi 讀取 MPU-6050 六軸感測器的資料。

加速規,又稱加速計、加速針、加速度傳感器、重力加速度傳感器等,是測量加速度的裝置。相對於遠距感測的裝置,它測量的是自身的運動。加速規的應用之一是測量重力,特別是使用於重量測定法的加速規上,這樣的裝置稱為重力計。[維基百科]

陀螺儀(英文:gyroscope),是一種用來感測與維持方向的裝置,基於角動量守恆的理論設計出來的。陀螺儀主要是由一個位於軸心且可旋轉的轉子構成。 陀螺儀一旦開始旋轉,由於轉子的角動量,陀螺儀有抗拒方向改變的趨向。陀螺儀多用於導航、定位等系統。[維基百科]


[材料]

• Raspberry Pi 2 Model B x 1
• MPU-6050模組 x 1
• 連接線 x 4條

[接線]

將 Raspberry Pi 的第 1 pin (GPIO4)接到MPU-6050的VCC,第 3 pin (GPIO4)接到MPU-6050的SDA。其他 pin 連接如下表:

MPU-6050模組
Raspberry pi接腳
VCC
1
SDA
3
SCL
5
GND
6

[設定步驟]

進入 root 模式執行設定,並開啟 SPI:
# raspi-config

選項如下:7.Advanced Options -> A7 I2C -> Enable -> YES -> OK -> <Finish>

檢查是否MPU-6050是否可以被偵測到,先安裝I2C工具:
$ sudo apt-get install i2c-tools

使用 Raspberry Pi A版的要使用以下指令:
$ sudo i2cdetect -y 0

使用 Raspberry Pi B版的要使用以下指令:
$ sudo i2cdetect -y 1

這時如果正常,會出現以下畫面:

為了要使用 python 讀取I2C,我們需要安裝 smbus 模組:
$ sudo apt-get install python-smbus

讀取加速度計及陀螺儀資料的程式(取自Bitify):

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

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

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    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)

bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

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

print "gyro data"
print "---------"

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)

print
print "accelerometer data"
print "------------------"

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled

print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

結果畫面如下:

[參考資料]



Share:

0 意見:

張貼留言