3-osiowy żyroskop i akcelerometr MPU6050

[tomaszdko]

Witam panowie.

Czy ktoś z Was bawił się MPU6050 ? W bascom trudno znaleźć kod taki co by zamieniał G na odchylenie kątowe w stopniach (na arduino jest cała masa ale tam mają biblioteki , podstawiasz zmienne i po sprawie).
Na razie rozpracowuję akcelerometr , właściwie chodzi ale nie do końca . Odchylając oś X pięknie oblicza do ale dochodząc do 90 stopni kąta pełnego oś Y zaczyna szaleć (pokazuje co chce przy małych odchyłach na osi Y).
Identycznie jest kiedy odchylam oś Y a X jest w miejscu i objaw ten sam przy 90 stopniach Y , X zaczyna szaleć . Kiedyś miałem moduł z adxl335 i było identycznie więc to na 100% wina w podejściu do kodu , stawiam na użycie funkcji ATN2(x,y) . Interesuje mnie też żyroskop i jak go obrobić ale najpierw możliwe poprawki z akcelerometrem.

Code: [Zaznacz cały] [Rozwiń/Zwiń]

1. $regfile = "m32def.dat"
2. $crystal = 20000000
3. $hwstack = 32
4. $swstack = 32
5. $framesize = 32
6. $baud =57600
7. '----------------------------------------------------------------------------------------------------------------
8. Config Lcd = 20 * 4
9. Config Lcdpin = Pin , Db4 = Portc.4 , Db5 = Portc.5 , Db6 = Portc.6 , Db7 = Portc.7 , E = Portc.3 , Rs = Portc.2
10. Cls
11. Cursor Off
12. '----------------------------------------------------------------------------------------------------------------
13. $lib "i2c_twi.lbx"
14. Config Scl = Portc.0
15. Config Sda = Portc.1
16. Config Twi = 400000
17. I2cinit
18. '----------------------------------------------------------------------------------------------------------------
19.  
20. Dim x As integer
21. Dim y As Integer
22. Dim z As Integer
23. Dim ay(2) As Byte
24. Dim az(2) As Byte
25. Dim ax(2) As Byte
26.  
27. dim xx as single,yy as single,zz as single ,dlax as single , dlay as single
28. dim xxx as single,yyy as single
29. dim textx as string*10,texty as string*10
30.  
31. '--- (25) Sample Rate Divider = 1 ---
32.    I2cstart                                                 'start condition
33.    I2cwbyte &HD0                                            'write adress of MPU-6050
34.    I2cwbyte 25                                              'Register 25 Sample Rate Divider (1..8 kHz)
35.    I2cwbyte &B00000000                                      'Divider set to 1   (soll)
36.    I2cstop                                                  'stop condition
37.  
38.    '--- (26) DLPF = 42/44 Hz ---
39.    I2cstart                                                 'start condition
40.    I2cwbyte &HD0                                            'write adress of MPU-6050
41.    I2cwbyte 26                                              'Register 26 DLPF_CFG (digital lowpass filter) Configuration
42.    I2cwbyte &B00000011                                      'Bits 0..2 = 011 (3) - ACC:44Hz, 4.9ms; Gyro:42Hz, 4.8ms
43.    I2cstop                                                  'stop condition
44.  
45.    '--- (27) Gyro Full Range = +-2000°/s ---
46.    I2cstart                                                 'start condition
47.    I2cwbyte &HD0                                            'write adress of MPU-6050
48.    I2cwbyte 27                                              'Register 27 Gyro Configuration
49.    I2cwbyte &B00011000                                      'Bits 3+4 = 11 - Full Scale Range: +/-2000°/ss
50.    I2cstop                                                  'stop condition
51.  
52.    '--- (28) ACC Full Range = +-2g ---
53.    I2cstart                                                 'start condition
54.    I2cwbyte &HD0                                            'write adress of MPU-6050
55.    I2cwbyte 28                                              'Register 28 ACC Configuration
56.    I2cwbyte &B00000000                                      'Bits 3+4 = 00 - Full Scale Range: +/-2g / No High Pass Filter
57.    I2cstop                                                  'stop condition
58.  
59.    '--- (107) Power Management 1 ---
60.    I2cstart                                                 'start condition
61.    I2cwbyte &HD0                                            'write adress of MPU-6050
62.    I2cwbyte 107                                             'Register 107 Power Management 1
63.    I2cwbyte &B00000011                                      'No Reset / No Sleep / No Cycle / Temp_Sens: Dis / Clock Source: Z-Gyro
64.    I2cstop
65.  
66. Do
67.  
68. gosub czytaj_xyz
69.  
70.      x=makeint(ax(1),ax(2))
71.      y=makeint(ay(1),ay(2))
72.      z=makeint(az(1),az(2))
73.      xx=x
74.      yy=y
75.      zz=z
76.      xx=x/16384
77.      yy=y/16384
78.      zz=z/16384
79.  
80.       xxx=atn2(xx,zz)
81.       yyy=atn2(yy,zz)
82.       dlax=RAD2DEG(yyy)
83.       dlay=RAD2DEG(xxx)
84.  
85.       textx = Fusing(dlax , "##.&")
86.       texty = Fusing(dlay , "##.&")
87.       locate 1,1
88.       lcd "x/ ";textx
89.       locate 2,1
90.       lcd "y/ ";texty
91.  
92. Loop
93. End
94.  
95. czytaj_xyz:
96.  
97. I2cstart
98.  
99.    I2cwbyte &HD0
100.    I2cwbyte 59
101.    I2crepstart
102.    I2cwbyte &HD1
103.  
104.    I2crbyte ax(2) , Ack
105.    I2crbyte ax(1) , Ack
106.  
107.    I2crbyte ay(2) , Ack
108.    I2crbyte ay(1) , Ack
109.  
110.    I2crbyte az(2) , Ack
111.    I2crbyte az(1) , Nack
112.  
113. I2cstop
114.  
115. return

chyba że ktoś ma gotowca to by mnie bardziej cieszyło .

[Oldman]

Cześć.
Może te "szprechanie" coś Ci pomoże: https://bascomforum.de/index.php?thread ... 0#post1647

[niveasoft]

Zrobiłem na tym czujniku taką huśtawkę i jak się przechyla to po niej wędrują diody WS8211.
Kiedy indziej zamontowałem go do skutera i jak jest zaparkowany a ktoś go poruszy to uruchamia alarm

Czujnik odczytuję cały czas i wyniki sobie uśredniam -> viewtopic.php?f=3&t=304

Co do Arduino to wystarczy sobie przetłumaczyć taką gotową bibliotekę. Tu problemu nie widzę

[tomaszdko]

Przeszedłem na adxl335 by na razie rozgryźć sam akcelerometr i można powiedzieć że jest ok . Do obsługi adxl335 używa się ADC . Mam jeszcze 2 płytki z ATXMEGA128a3 (kiedyś je kupiłem ale nie używałem) . Ma ktoś kawałek kodu jak ustawić w BASCOM ADC jako single ended , tak jak przy typowej atmedze 0...5v . Jest też potrzebna kalibracja tego ADC bo nie będzie pomiaru od 0V.

[tomaszdko]

ok akcelerometr rozgryziony jak i adxl335 oraz mpu6050 , ładnie widać wizualizację na PROCESSING 3D . Czas na żyroskop . Mam kod z arduino ale jakoś nie mogę zatrybić pewnej kwestii . Chodzi zamianę surowych danych na kąt .

Wyciągam surowe dane i dziele przez 32.8 i dalej nie mogę zatrybić jak to zamienić na stopnie.

To jest kod w arduino :

Code: [Zaznacz cały] [Rozwiń/Zwiń]

1. /* http://www.youtube.com/c/electronoobs
2.  *
3.  * This is an example where we configure te data of the MPU6050
4.  * and read the gyro data and print it to the serial monitor
5.  *
6.  * Arduino pin    |   MPU6050
7.  * 5V             |   Vcc
8.  * GND            |   GND
9.  * A4             |   SDA
10.  * A5             |   SCL
11.  */
12.  
13. //Includes
14. #include <Wire.h>
15.  
16.  
17. //Variables
18. float elapsedTime, time, timePrev;        //Variables for time control
19. int gyro_error=0;                         //We use this variable to only calculate once the gyro data error
20. int16_t Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data read
21. float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value obtained with Gyro data
22. float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error
23.  
24.  
25.  
26. void setup() {
27.   Wire.begin();                           //begin the wire comunication  
28.   Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)              
29.   Wire.write(0x6B);                       //make the reset (place a 0 into the 6B register)
30.   Wire.write(0x00);
31.   Wire.endTransmission(true);             //end the transmission
32.  
33.   Wire.beginTransmission(0x68);           //begin, Send the slave adress (in this case 68)
34.   Wire.write(0x1B);                       //We want to write to the GYRO_CONFIG register (1B hex)
35.   Wire.write(0x10);                       //Set the register bits as 00010000 (1000dps full scale)
36.   Wire.endTransmission(true);             //End the transmission with the gyro
37.  
38.   Serial.begin(9600);                     //Remember to set this same baud rate to the serial monitor  
39.   time = millis();                        //Start counting time in milliseconds
40.  
41.  
42.  
43. /*Here we calculate the gyro data error before we start the loop
44.  * I make the mean of 200 values, that should be enough*/
45.   if(gyro_error==0)
46.   {
47.     for(int i=0; i<200; i++)
48.     {
49.       Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68)
50.       Wire.write(0x43);                        //First adress of the Gyro data
51.       Wire.endTransmission(false);
52.       Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers
53.          
54.       Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
55.       Gyr_rawY=Wire.read()<<8|Wire.read();
56.    
57.       /*---X---*/
58.       Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8);
59.       /*---Y---*/
60.       Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
61.       if(i==199)
62.       {
63.         Gyro_raw_error_x = Gyro_raw_error_x/200;
64.         Gyro_raw_error_y = Gyro_raw_error_y/200;
65.         gyro_error=1;
66.       }
67.     }
68.   }  
69. }//end of setup void
70.  
71.  
72.  
73.  
74.  
75.  
76. void loop() {
77.     timePrev = time;                        // the previous time is stored before the actual time read
78.     time = millis();                        // actual time read
79.     elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds
80.  
81.    
82. //////////////////////////////////////Gyro read/////////////////////////////////////
83.  
84.     Wire.beginTransmission(0x68);            //begin, Send the slave adress (in this case 68)
85.     Wire.write(0x43);                        //First adress of the Gyro data
86.     Wire.endTransmission(false);
87.     Wire.requestFrom(0x68,4,true);           //We ask for just 4 registers
88.        
89.     Gyr_rawX=Wire.read()<<8|Wire.read();     //Once again we shif and sum
90.     Gyr_rawY=Wire.read()<<8|Wire.read();
91.     /*Now in order to obtain the gyro data in degrees/seconds we have to divide first
92.     the raw value by 32.8 because that's the value that the datasheet gives us for a 1000dps range*/
93.     /*---X---*/
94.     Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x;
95.     /*---Y---*/
96.     Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;
97.    
98.     /*Now we integrate the raw value in degrees per seconds in order to obtain the angle
99.     * If you multiply degrees/seconds by seconds you obtain degrees */
100.     /*---X---*/
101.     Gyro_angle_x = Gyro_angle_x  + Gyr_rawX*elapsedTime;
102.     /*---X---*/
103.     Gyro_angle_y = Gyro_angle_y  + Gyr_rawY*elapsedTime;
104.  
105.     //Serial.print("GyroX raw: ");
106.     //Serial.print(Gyr_rawX);
107.     //Serial.print("   |   ");
108.     Serial.print("GyroY raw: ");
109.     Serial.println(Gyr_rawY);
110.     //Serial.print("   |   ");
111.     //Serial.print("GyroX angle: ");
112.     //Serial.print(Gyro_angle_x);
113.     //Serial.print("   |   ");
114.     //Serial.print("GyroY angle: ");
115.     //Serial.println(Gyro_angle_y);
116.  
117. }

a mnie interesuje ta część :

/*Now we integrate the raw value in degrees per seconds in order to obtain the angle
* If you multiply degrees/seconds by seconds you obtain degrees */
/*---X---*/
Gyro_angle_x = Gyro_angle_x + Gyr_rawX*elapsedTime;
/*---X---*/
Gyro_angle_y = Gyro_angle_y + Gyr_rawY*elapsedTime;

i ta wskazująca czas :

timePrev = time; // the previous time is stored before the actual time read
time = millis(); // actual time read
elapsedTime = (time - timePrev) / 1000; //divide by 1000 in order to obtain seconds

może ktoś to mi przetłumaczyć ? co i jak zrobić.

[niveasoft]

millis() to funkcja wbudowana w Arduino która odmierza czas co 1ms. Dword przepełnia się co około 50 dni.
Chociaż jedno jej użycie w kodzie powoduje zaimplementowanie całego potrzebnego kodu. Nie zagłębiałem sie ale ona chyba zajmuje jeden timer i sama włącza przerwania od tego timera oraz globalne.
Jako że funkcje zegara w Bascom są z dokładnością do 1s to się nie nadadzą i musisz to zrobić na piechotę. Czyli musisz ustawić jakiś timer na 1ms i w przerwaniu zwiększać zmienną typu Dword.
Z drugiej strony to widzę, że w tym kodzie chodzi im o sekundy... Tak czy siak musisz zacząć mierzyć czas.

PS. Proszę Cię zwróć uwagę na to jak powinien być umieszczany kod na Forum -> viewtopic.php?f=3&t=107
Jest kolorowany i można go potem rozwijać a nie tylko przewijać. Podmieniłem kodowanie i dla Arduino wybrałem CPP, ale może C byłoby lepsze

[tomaszdko]

Dzięki , kierunek dobry . Narazie zawiesiłem projekt z żyro , pozostałem przy akcelerometrze .....Siedzę nad XMEGA , taki fajny procek (potencjał ogromny) a tyle z nim problemów , mało kodów a samo tłumaczenie przez ludzi tworzących bascoma to kpina (ale nawet w C też mało ludzi siedzi) i nie dziwi mnie to że ten procek nie zrobił furory ! milion ustawień do durnych rzeczy . Poległem na prostej sprawie ! Ustawienie pinu na input (to nie problem) ale podciagniecie na - czy + to komedia (zmienia się jak w kalejdoskopie , jak by wisiało wejście) , dam sobie luz ..adc 12bit to na i2c pociągnę z ads1115 (szkoda bo xmega ma możliwość kanałów dma no i da się kręcić bez problemów do 50Mhz)

[niveasoft]

Mamy tu na Forum dział Xmega - wystarczy zapytać.
Osobiście mam kilka mocno rozbudowanych projektów na Xmega`ch tylko brakuje czasu żeby tak samemu od siebie wszystko opisywać.

Tu kawałek kodu dla prostej klawiaturki:

Code: [Zaznacz cały] [Rozwiń/Zwiń]

1. '*****************************************************************
2. '*                      I/O CONFIG                               *
3. '*****************************************************************
4. Config Portb.0 = Input : Sw_up Alias Pinb.0
5.  Config Xpin = Portb.0 , Outpull = Pullup , Sense = Falling
6. Config Portb.4 = Input : Sw_okay Alias Pinb.4
7.  Config Xpin = Portb.4 , Outpull = Pullup , Sense = Falling
8. Config Portb.3 = Input : Sw_right Alias Pinb.3
9.  Config Xpin = Portb.3 , Outpull = Pullup , Sense = Falling
10. Config Portb.2 = Input : Sw_down Alias Pinb.2
11.  Config Xpin = Portb.2 , Outpull = Pullup , Sense = Falling
12. Config Portb.1 = Input : Sw_left Alias Pinb.1
13.  Config Xpin = Portb.1 , Outpull = Pullup , Sense = Falling

PS. Pamietaj o funkcji Bascom`a ->Intelli sense ->podpowiada co wpisać w Config jak klikniesz CTRL+SPACE

[tomaszdko]

fakt chodzi na tym co pokazałeś a czego nie znajdziesz w AN#192-3.pdf , ale i tak zrazilem się ostro jak wielu innych do xmegi np:

Kod: Zaznacz cały

dim a as byte
CONFIG portb=output
CONFIG XPIN=PORTb,OUTPULL=PULLUP

Do
a=a+1
locate 1,1
lcd a

 toggle portb
 waitms 100
Loop

Piny od 0 do 3 faktycznie zmieniają stan a reszta nie (na ATmegach nie ma problemu ) , Ja mam problem z pin i port a nawet nie chce wiedzieć co bedzie z timer1 nie wspomne o adc i dma! tam jest milion rejestrów (ich kombinacji)! Jeśli wpiszesz w google "adc bascom" to masz kilka , dosłownie kilka wyników a w C jest tylko trochę więcej i dlatego ten procesor nie przebił się ...odpuszczam sobie i wracam do ATmega128 ,1284 lub 32 a xmega idzie w śmieci bo nie chce sie irytować.