MPU6050

Przykład prostego programu który ramieniem dołączonym do serwa podąża za ruchem czujnika

Program jest przykładowy

$regfile = "m8def.dat"
$crystal = 16000000
$hwstack = 40
$swstack = 16
$framesize = 32

Config Submode = New

$lib "i2c_twi.lib"                                          'używamy biblioteki do sprzętowego TWI (I2C)
 Config Scl = Portc.5                                       'określanie pinów żeby biblioteka wiedziała które
 Config Sda = Portc.4
  I2cinit                                                   'ustawia piny w pożądany stan

   Config Twi = 400000                                      'ustawia predkośc na 400kHz


Dim Servo_val As Byte                                       'zmienna trzyma wartość wypełnienia dla serwa
Dim New_x As Byte                                           'odczytana z żyroskopu pozycja względem osi X
Dim Help_w As Word                                          'zmienna pomocnicza do obliczeń
Dim New_val As Word                                         'zmienna żeby zaznaczyć w kodzie gdzie powstaje nowa wartość
Dim Old_val As Byte                                         'poprzednia wartość do porówniania

Config Portb.1 = Output                                     'ustawia port dla diody jako wyjściowy,
 Led Alias Portb.1                                          'nadawanie przyjaznej nazwy dla pinu portu

Config Servos = 1 , Servo1 = Portd.2 , Reload = 10          'konfiguracja 1 serwa na PORTD.2

Config Portd.2 = Output                                     'ustawienie pinu portu jako wyjscie (dla serwa)

'skrajne połozenia serwa przy których nie dobija do końca powodując przeciążenie to 45 min i 165 max
'  165 - 45 = 120        kroków od jednego do drugiego skrajnego jest zatem 120
' czyli środkowe położenie to 60 + 45 = 115   (60 środek + minimum gdzie serwo sie zatrzymuje)

Const Polozenie_srodkowe = 60 + 45                          'kompilator to sam wyliczy (105)


' Z żyroskopu wartosci odczytywane są nie tak "zwyczajnie"
' Kiedy płytka jest ustawiona horyzontalnie i przechylasz DO GÓRY to wartośc odczytywana zmienia sie od 0 do 64    ( rośnie )
' Kiedy płytka jest ustawiona horyzontalnie i przechylasz  W DÓŁ  to wartosć odczytywana zmienia się od 255 do 193 ( zmniejsza sie )
' Od horyzontalniej do pionowej jest kąt prosty czyli 90 stopni
' na 90stopni są 64 kroki (wartość odczytywana)
' na 90stopni serva sa 60 kroki

'przelicznik 64/60 = 1,066


Enable Interrupts

'***********************************************************
'*      TEN SUB ZAINICJUJE I POUSTAWIA ŻYROSKOP            *
'***********************************************************
Sub Init_mpu

'--- (25) Sample Rate Divider = 1 ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 25                                              'Register 25 Sample Rate Divider (1..8 kHz)
   I2cwbyte &B00000000                                      'Divider set to 1   (soll)
   I2cstop                                                  'stop condition

   '--- (26) DLPF = 42/44 Hz ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 26                                              'Register 26 DLPF_CFG (digital lowpass filter) Configuration
   I2cwbyte &B00000011                                      'Bits 0..2 = 011 (3) - ACC:44Hz, 4.9ms; Gyro:42Hz, 4.8ms
   I2cstop                                                  'stop condition

   '--- (27) Gyro Full Range = +-2000°/s ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 27                                              'Register 27 Gyro Configuration
   I2cwbyte &B00011000                                      'Bits 3+4 = 11 - Full Scale Range: +/-2000°/ss
   I2cstop                                                  'stop condition

   '--- (28) ACC Full Range = +-2g ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 28                                              'Register 28 ACC Configuration
   I2cwbyte &B00000000                                      'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
   I2cstop                                                  'stop condition

   '--- (107) Power Management 1 ---
   I2cstart                                                 'start condition
   I2cwbyte &HD0                                            'write adress of MPU-6050
   I2cwbyte 107                                             'Register 107 Power Management 1
   I2cwbyte &B00001011                                      'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
   I2cstop

End Sub


'************************************************************
'*   TEN SUB WYKONA RUCH SERWA TAM I Z POWROTEM (TEST)      *
'************************************************************
Sub Servo_test

 Waitms 100

 For Servo_val = 45 To 165                                  'od minimu do maksimum

  Servo(1) = Servo_val                                      'ustaw serwo
   Waitms 5                                                 'i poczekaj 5ms

 Next

 Waitms 100

 For Servo_val = 165 To 45 Step - 1

  Servo(1) = Servo_val
   Waitms 5

 Next

End Sub



Call Servo_test                                             ' na początku serwo wykona ruchy w skrajne położenia


Servo_val = Polozenie_srodkowe
 Servo(1) = Servo_val

  Call Init_mpu

Do

    I2cstart                                                'start magistrali I2C
     I2cwbyte &HD0                                          'wpisuje adres żyroskopu do zapisu
     I2cwbyte 59                                            'wybieram rejestr który chcę czytać
     I2crepstart                                            'ponawiam start (wymagane przez żyroskop)
     I2cwbyte &HD1                                          ' wpisuje adres żyroskopu do odczytu

      I2crbyte New_x , Nack                                 'czytam wybrany bajt

    I2cstop                                                 'zakonczenie pracy z magistralą


   'wykonaj operacje w zalezności od tego jaką wartość ma odczytana zmienna

    Select Case New_x

     Case 0                                                 'przy 0 i 255 serwo jest na środku
       New_val = Polozenie_srodkowe
     'czyli serwo jest wysuniete już w połowie

     'kiedy odchylasz do góry to trzeba odjąć wartośc z serwa żeby się "schowało"
     Case 1 To 64
       Help_w = 64 - New_x                                  
       Help_w = Help_w * 100                                
       Help_w = Help_w / 106                                '100/106 to przełozenie ruchu serwa w stosunku do żyro
        New_val = 45 + Help_w
       'serwo ma minimalną wartość 45 wiec obliczone wychylenie dodajemy do minimum

      'kiedy odchylasz na dół wartośc nagle z 0 robi sie 255 i potem spada
     Case 193 To 254
       Help_w = 255 - New_x                                 
       Help_w = Help_w * 100                                
       Help_w = Help_w / 106
        New_val = 105 + Help_w
                    '105 = 60 od srodka + 45 przesuniecie

     Case 255
       New_val = Polozenie_srodkowe


    End Select

     '*******************************************************
     '* TUTAJ DODAJE DO SIEBIE NOWA WARTOŚĆ DO POPRZEDNIEJ  *
     '*  I DZIELE PRZEZ DWA, TAKIE UŚREDNIANIE POWODUJE     *
     '*      ŁAGODNIEJSZE REAKCJE SERWA NA ZMIANY           *
     '*******************************************************

       Help_w = Servo_val                                   'pobranie starej wartosci do zmiennej
       Help_w = Help_w + New_val                            'dodanie nowej wartosci
        Shift Help_w , Right , 1                            ' Shift 1 powoduje podział przez 2
         Servo_val = Help_w                                 ' zapis nowej zmiennej



       If Old_val <> Servo_val Then                         'jesli nowa wartość jest inna/różna od poprzedniej
           Old_val = Servo_val                              'to zapamietaj nową

           Reset Led                                        'ale włącz diodę (ruch, zmiany)

           Servo(1) = Servo_val                             'i wpisz nową wartośc dla serwa

       Else                                                 'jesli jednak nowa wartość jest taka sama (brak ruchu)

           Set Led                                          'to zgaś LED

       End If


     Waitms 25                                              'krótkie opóźnienie (można spróbować pozmieniać lub usunąć)

Loop
End

Gdyby ktoś chciał odczytać wszystkie wartości to pokażę jak to zrobić łatwo, szybko i przyjemnie

Za pomocą I2creceive wciągamy 14 bajtów a dlatego ze na tej tablicy zbudowałem "strukturę" to w programie mogę odwoływać się bezpośrednio do gotowych wartości typu Integer

Dim Rbytes(14) As Byte

Dim X_accel As Integer At Rbytes(1) Overlay
Dim Y_accel As Integer At Rbytes(3) Overlay
Dim Z_accel As Integer At Rbytes(5) Overlay
Dim Temp As Integer At Rbytes(7) Overlay
Dim X_gyro As Integer At Rbytes(9) Overlay
Dim Y_gyro As Integer At Rbytes(11) Overlay
Dim Z_gyro As Integer At Rbytes(13) Overlay

Const Mpu_i2c_address = &HD0 

 Rbytes(1) = 59
      I2creceive Mpu_i2c_address , Rbytes(1) , 1 , 14
	  Swap X_accel : Swap Y_accel : Swap Z_accel : Swap Temp : Swap X_gyro : Swap Y_gyro : Swap Z_gyro