Взаємодія Raspberry Pi та інерційно-вимірювального датчика MPU-6050
Оценка пользователей: / 9
ПлохоОтлично 
There are no translations available.

IMU-6050Розглянемо взаємодію Pi з шести осевим гіроскопом + акселерометром, зупинившись на мікросхемі MPU-6050, яка не лише дешева, але й одна з самих надійних і точних.
IMU-6050 board
Виводи MPU 6050 для під’єднання до Raspberry Pi
Налаштування (для Rasbian)
Цей датчик має інтерфейс I2C, тому спочатку необхідно встановити відповідні драйвери Linux. Відкрийте файл для редагування (потрібно sudo)
sudo nano /etc/modules
додайте наступні рядки в кінець файлу, збережіть його і перезавантажте Pi:
i2c-bcm2708
i2c-dev
Тепер перевірте файл чорних списків:
sudo nano /etc/modprobe.d/raspi-blacklist.conf
і переконайтеся, що наступні рядки починаються з символу # (закоментовані), якщо вони там знаходяться, щоб не не турбуватися:
#blacklist spi-bcm2708
#blacklist i2c-bcm2708
Raspberry Pi with IMU-6050
Під'єднання контактів MPU-6050 до Raspberry Pi
Підключення датчика
Для підключення датчика нам необхідно використати виводи GPIO на Pi таким чином:
• Pin 1 - 3,3 підключитися до VCC
• Pin 3 - SDA підключення до SDA
• Pin 5 - SCL підключення до лінії SCL
• Pin 6 – «Землю» підключити до GND
Підключення виводів показане на малюнку вище.
Після того, як підключили плату, можете перевірити, чи Pi виявив її. Це робиться за допомогою наступної команди для встановлення інструментів I2C:
sudo apt-get install i2c-tools
а потім або
sudo i2cdetect -y 0 (для Rev. 1 плати)
або
sudo i2cdetect -y 1 (для Rev. 2 плати)
і ви повинні побачити схему приєднання будь-яких пристроїв I2C та їх адреси:
I2C - IMU6050
Приєднані пристрої I2C та їх адреси
Бачимо, що Pi виявив датчик з адресою 0x68 (HEX-код) - ця адреса необхідна для взаємодії з ним. Введіть таку команду і ви повинні отримати виведене 0x68 на екрані, якщо все працює правильно:
sudo i2cget -y 0 0x68 0x75
Ця команда звертається до пристрою, адреса якого 0x68 (датчик) і повертає значення в регістрі 0x75, який має за замовчуванням значення 0x68 - те ж значення, що і адреса.
Читання даних з MPU-6050 на Raspberry Pi
Ми підключили датчик акселерометра і гіроскопа до Raspberry Pi, а тепер розглянемо простий код Python для читання даних, які надає датчик.
Щоб мати можливість читати з I2C з використанням Python ми повинні встановити для шини модуль smbus:
sudo apt-get install python-smbus
Тепер введемо не щось конкретне, а просто довільний тестовий код, щоб переконатися, що датчик працює:
#!/usr/bin/python

import smbus
import math

# Регістри управління живленням
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) # або bus = smbus.SMBus(1) для плати Rev. 2
address = 0x68       # Це значення адреси отримане командою i2cdetect

# Тепер розбудимо 6050, тому що це запускається в режимі сну
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)
Коли ви введете код, то побачите результат, схожий на таке:
Gyro_1
Дані акселерометра
Давайте подивимося на код більш докладно.
accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)
Три рядки читають сирі значення X, Y і Z акселерометра, параметр кожного виклику є реєстрацією в датчика, який містить дані. Датчик має ряд регістрів з різною функціональністю, як описано в даному даташит. Адреси регістрів з даними акселерометра, які цікавлять нас, є 0x3b, 0x3d, 0x3f, і вони тримають сирі дані у форматі 16 біт доповнюючого двійкового коду.
Наступний код читає слово (16 біт) з даного реєстру і перетворює його в двійковий доповнюючий код:
def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val
Як тільки ми отримаємо вихідні дані, ми повинні масштабувати їх, а потім перетворити в щось корисне, наприклад, кут обертання. Знову ж з даташит ми бачимо, що для  масштабування за замовчуванням ми повинні застосувати до вихідних значень акселерометра 16384, тому ми ділимо вихідні дані на цю величину.
accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0
Тепер у нас є значення, які гравітація генерує на датчику в кожному з трьох вимірів, а з цього ми можемо обчислити повороти в осях X і Y.
def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_x_rotation(x,y,z):
    radians = math.atan(x / dist(y,z))
    return math.degrees(radians)

def get_y_rotation(x,y,z):
    radians = math.atan(y / dist(x,z))
    return math.degrees(radians)
Ось чудова стаття, що показує математичні деталі цього. Те, що це нам дає, є кутом повороту в градусах для обох осей X і Y і показано у виведенні:
Gyro_2
Таким чином, в цьому випадку датчик повернувся на -13.7 градуса навколо X і на -0.1 градуса навколо Y.
Дані гіроскопа
Аналогічним чином, можемо прочитати дані з гіроскопної частини датчика. Це робиться в наступному коді.
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)
Так, ми читаємо значення з регістрів 0x43, 0x45 і 0x47, знову бачимо з таблиці, що в них знаходяться вихідні дані гіроскопа. Для масштабування ми ділимо їх на 131, щоб отримати величину обертання в градусах в секунду.
Gyro_4
В нашому випадку вихідні дані показали, що гіроскоп не рухався, коли його значення читали.
Представлений тут код дуже простий і повинен бути розширений для обробки помилок і щоб дозволяв налаштовувати датчик з різними рівнями чутливості. Автор зробив це в своєму додатку і вбудував його в веб-сервер. Це дозволяє зробити простий запит HTTP до Raspberry Pi і отримати читання з датчика.
Щоб допомогти в тестуванні і краще візуалізувати дані, автор запропонував кілька простих OpenGL кодів для графічного представлення орієнтації датчика в 3D-просторі.
3D OpenGL візуалізація даних з MPU-6050, підключеного до Raspberry Pi
Отримання даних за допомогою простого веб-сервера
Давайте почнемо зі створення простої сервер на основі web.py, який встановлюється за допомогою apt-get:
sudo apt-get install python-webpy
Тепер створіть каталог, щоб помістити в нього код в і створити просту тестову програму:
mkdir webpy
cd webpy
sudo nano server.py
Використайте наступне як тест:
#!/usr/bin/python
import web

urls = (
    '/', 'index'
)

class index:
    def GET(self):
        return "Hello, world!"

if __name__ == "__main__":
    app = web.application(urls, globals())
    app.run()
Збережіть, а потім встановіть його як виконуваний з:
chmod +x server.py
Потім запустіть код:
./server.py
Ви побачите щось на зразок цього, що показує, що сервер чекає запиту (натискання Ctrl + C зупиняє сервер)
Gyro_3
Тепер направте свій браузер за адресою //IP-адреса-вашого-pi: 8080 і він покаже веб-сторінку із вмістом Hello, World!. Ми можемо використати це, щоб зчитувати дані з віддаленого комп'ютера, наприклад, з робочий стола Linux.
Додавання коду датчика на сервер
Замініть вміст server.py на такий:
#!/usr/bin/python
import web
import smbus
import math

urls = (
    '/', 'index'
)
# Регістри управління живленням
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

bus = smbus.SMBus(0) # або bus = smbus.SMBus(1) для плати Rev. 2
address = 0x68              # Це значення адреси, прочитане командою i2cdetect

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)

class index:
    def GET(self):
        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
        return str(get_x_rotation(accel_xout_scaled, accel_yout_scaled,
accel_zout_scaled))+" "+str(get_y_rotation(accel_xout_scaled,
accel_yout_scaled, accel_zout_scaled))
if __name__ == "__main__":

    # Тепер розбудимо 6050, тому що це запускається в режимі сну
    bus.write_byte_data(address, power_mgmt_1, 0)
    app = web.application(urls, globals())
    app.run()
Сервер повинен бути запущений з sudo, щоб мати права на читання з I2C:
sudo ./server.py
Підключення через ваш браузер тепер буде повертати значення обертання X & Y, наприклад,
Gyro_5
3D візуалізація
Автор спробував код OpenGL, щоб отримати щось візуальне.
Налаштування OpenGL і Pygame
sudo apt-get install python-opengl
sudo apt-get install python-pygame
Тепер збережіть наступне в файл (наприклад, level.py) і запустіть його:
#!/usr/bin/python

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

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://192.168.1.65:8080" # Замініть цю адресу вашою
    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()
    cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7))
    angle = 0
    
    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)

        for x in range(-20, 22, 2):
            glVertex3f(x/10.,-1,-1)
            glVertex3f(x/10.,-1,1)
        
        for x in range(-20, 22, 2):
            glVertex3f(x/10.,-1, 1)
            glVertex3f(x/10., 1, 1)
        
        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z/10.)
            glVertex3f( 2, -1, z/10.)

        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z/10.)
            glVertex3f(-2,  1, z/10.)

        for z in range(-10, 12, 2):
            glVertex3f( 2, -1, z/10.)
            glVertex3f( 2,  1, z/10.)

        for y in range(-10, 12, 2):
            glVertex3f(-2, y/10., 1)
            glVertex3f( 2, y/10., 1)
        
        for y in range(-10, 12, 2):
            glVertex3f(-2, y/10., 1)
            glVertex3f(-2, y/10., -1)
        
        for y in range(-10, 12, 2):
            glVertex3f(2, y/10., 1)
            glVertex3f(2, y/10., -1)
        
        glEnd()
        glPushMatrix()
        glRotate(float(x_angle), 1, 0, 0)
        glRotate(-float(y_angle), 0, 0, 1)
        cube.render()
        glPopMatrix()
        pygame.display.flip()

class Cube(object):

    def __init__(self, position, color):
        self.position = position
        self.color = color

    # інфоррмація про куб
    num_faces = 6

    vertices = [ (-1.0, -0.05, 0.5),
                 (1.0, -0.05, 0.5),
                 (1.0, 0.05, 0.5),
                 (-1.0, 0.05, 0.5),
                 (-1.0, -0.05, -0.5),
                 (1.0, -0.05, -0.5),
                 (1.0, 0.05, -0.5),
                 (-1.0, 0.05, -0.5) ]

    normals = [ (0.0, 0.0, +1.0),  # front
                (0.0, 0.0, -1.0),  # back
                (+1.0, 0.0, 0.0),  # right
                (-1.0, 0.0, 0.0),  # left
                (0.0, +1.0, 0.0),  # top
                (0.0, -1.0, 0.0) ]  # bottom

    vertex_indices = [ (0, 1, 2, 3),  # front
                       (4, 5, 6, 7),  # back
                       (1, 5, 6, 2),  # right
                       (0, 4, 7, 3),  # left
                       (3, 2, 6, 7),  # top
                       (0, 1, 5, 4) ]  # bottom

    def render(self):
        then = pygame.time.get_ticks()
        glColor(self.color)

        vertices = self.vertices

        # Draw all 6 faces of the cube
        glBegin(GL_QUADS)

        for face_no in xrange(self.num_faces):
            glNormal3dv(self.normals[face_no])
            v1, v2, v3, v4 = self.vertex_indices[face_no]
            glVertex(vertices[v1])
            glVertex(vertices[v2])
            glVertex(vertices[v3])
            glVertex(vertices[v4])
        glEnd()

if __name__ == "__main__":
    run()
Не забутьте змінити URL в рядку 038 вашим конкретним значенням (адреса, яку ви використовували раніше для тестування сервера). Коли ви запустите код, то відкриється вікно орієнтації датчика: повертаючи датчик, екран оновиться.
3D Model IMU-6050
3D модель руху датчика
Ви помітите, що навіть, коли датчик фізично не переміщується, дані шуму змушуючи його коливатися. В одній з наступних статей розглянемо, як зменшити це.
(Джерело EN: blog.bitify.co.uk)
 
>
КнигаНовости Практика поискаПартнерыО нас
Підтримка та дизайн: Могильний С.С. Шаблон: Joomla Templates by BuyHTTP Joomla Hosting