MPU6050 na Raspberry pi w Python'ie, czyli żyrokompas i akceleromentr w jednym, na malinie


Kto miał pomysł aby zrobić quadrocopter na raspberry pi?? A może ktoś zdecydował się zrobić robota samopoziomującego się? I do tego aby był on autonomiczny, czyli żeby sam się sterował? Do tego niezbędne będzie zastosowanie żyroskopu i akcelerometru.



Na powyższym zdjęciu możecie zobaczyć jak wygląda MPU6050, jaki jest mały. Gdy go kupujemy, przychodzi w dwóch częściach, tzn. płytka i oddzielnie goldpin'y, które trzeba przylutować. A to jak widać po lutach jakie udało mi sięwykonać może być wymagające. Dopowiem, że dla upartyh lutownica transformatorowa wystarczy, ale lepiej sprawdza się jednak grotowa.

Oczywiście można wczytywać się w dokumentację czujnika, ale jeżeli chcemy podłączyć czujnik do maliny wystarczy, żę połączycie to jak na poniższym rysunku.


Gdy już udało nam się podłączyć, co potwierdzi zielona dioda na czujniku, możemy sprawdzić czy malina widzi czujnik, czyli czy komunikacja z czujnikiem po magistrali i2c działa. W tym celu wpisujemy komendę:

i2cdetect -y 1

W efekcie powinna się pojawić tabela, na której wyświetli się adres 68. Na poniższym rysunku wyświetliły się trzy adresy, ponieważ gdy robiłem zrzut ekranu miałem podpięte trzy różne czujniki. Na czerwono oznaczyłem ten odpowiadający za MPU6050.



W Python'ie będziemy się odwoływać do adresu "0x68", ponieważ w taki sposób oznacza się liczby w systemie szesnastkowym.

Jeżeli już wiemy, że malina komunikuje się z czujnikiem możemy przejść do pisania programu w Python'ie. Poniżej gotowy, kompletny kod z komentarzami, która komenda za co odpowiada. Można śmiało kopiować i wrzucać do siebie.

import smbus # importujemy blibliotekę odpowiedzialną za komunikację z czujnikiem
import math  # importujemy bibliotekę "matematyczną"

power_mgmt_1=0x6b # do zmiennej podajemy nr rejestru wzbudzenia czujnika

def read_byte(adr): # funkcja odczytu pojedynczego byte'u
    print(bus.read_byte_data(address, adr))
    return bus.read_byte_data(address, adr)

def read_word(adr): # funkcja przetwarzająca dwa koljne byte'y w wielkość 16 - bitową
    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): # funkcja przeliczająca wartość binarną na dziesiętną. 
    val = read_word(adr)
    if(val >= 0x8000): # najstarszy bit mówi czy liczba jest dodatnia czy ujemna
        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(1) # aktywujemy komunikację w opraciu o smbus
address = 0x68
bus.write_byte_data(address, power_mgmt_1, 0) # wzbudzamy czujnik do działania


while True:
    print "gyro data"
    print "---------"
    gyro_xout = read_word_2c(0X43)	# odczyt z konkretnych numerów rejestru - żyroskop
    gyro_yout = read_word_2c(0X45)
    gyro_zout = read_word_2c(0X47)
    print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131) # matematyka opisana w dokumentacji czujnika
    print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
    print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)
    print "accelerometer data"
    print "------------------"
    accel_xout = read_word_2c(0x3b) # odczyt z konkretnych numerów rejestru - akcelerometr
    accel_yout = read_word_2c(0x3d)
    accel_zout = read_word_2c(0x3f)
    accel_xout_scaled = accel_xout / 16383.0 # matematyka opisana w dokumentacji czujnika
    accel_yout_scaled = accel_yout / 16383.0
    accel_zout_scaled = accel_zout / 16383.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
       
    x=get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
    print "x_rotation: ",x
    y=get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
    print "y_rotation: ", y
    print"***************************"
	


To na co należy zwrócić uwagę, to to że program oddzielnie drukuje (print) wartości odczytane z żyroskopu i akcelerometru. W praktyce jest to niewystarczające ponieważ ani żyroskop, ani akcelerometr nie są w pojedynkę wystarczająco precyzyjne. Dlatego wyniki z odczytu obu kompiluje się za pomocą filtru Kalmana albo filtru komplementarnego. Ale jak to się robi opiszę już w oddzielnych wpisach. Mam nadzieję, że powyższe pomogło odpalić własny gyro i akcelerometr. :) Pozdrawiam. :)

:)