陀螺儀積分的問題

Bhai

我目前在用 Arduino 做四軸，
我用的是 L3G4200D 的陀螺儀，
但是我在做積分時，陀螺儀靜止時候的角度也會一直加上去，
請問...這該怎麼解決?


下面是我用 L3G4200D 求角度的程式。

1. #include <Wire.h>
   
2. 
   
3. #define CTRL_REG1 0x20
   
4. #define CTRL_REG2 0x21
   
5. #define CTRL_REG3 0x22
   
6. #define CTRL_REG4 0x23
   
7. #define CTRL_REG5 0x24
   
8. int L3G4200D_Address = 105; //I2C address of the L3G4200D
   
9. 
   
10. int x;
    
11. int y;
    
12. int z;
    
13. float angleG;
    
14. float gyroSpeed;
    
15. unsigned long o_timer, timer;
    
16. 
    
17. void setup(){
    
18.   Wire.begin();
    
19.   Serial.begin(9600);
    
20. 
    
21.   Serial.println("starting up L3G4200D");
    
22.   setupL3G4200D(250); // Configure L3G4200  - 250, 500 or 2000 deg/sec
    
23.   angleG = 0;
    
24.   gyroSpeed = 0;
    
25.   o_timer = 0;
    
26.   timer = 0;
    
27.   delay(1500); //wait for the sensor to be ready
    
28.   timer = millis();
    
29. }
    
30. 
    
31. void loop()  {
    
32.   long o_timer = timer;
    
33.   getGyroValues();  // This will update x, y, and z with new values
    
34.   timer = millis();
    
35.   int dt = timer - o_timer;
    
36.   gyroSpeed = x * 8.75 / 1000;
    
37.   angleG = angleG + gyroSpeed * dt / 1000;
    
38.   Serial.print("angle_x: ");
    
39.   Serial.println(angleG,6);
    
40.   
    
41.   //delay(100); //Just here to slow down the serial to make it more readable
    
42. }
    
43. 
    
44. void getGyroValues(){
    
45.   //(deg/sec)
    
46.   byte xMSB = readRegister(L3G4200D_Address, 0x29);
    
47.   byte xLSB = readRegister(L3G4200D_Address, 0x28);
    
48.   x = ((xMSB << 8) | xLSB);
    
49. 
    
50.   byte yMSB = readRegister(L3G4200D_Address, 0x2B);
    
51.   byte yLSB = readRegister(L3G4200D_Address, 0x2A);
    
52.   y = ((yMSB << 8) | yLSB);
    
53. 
    
54.   byte zMSB = readRegister(L3G4200D_Address, 0x2D);
    
55.   byte zLSB = readRegister(L3G4200D_Address, 0x2C);
    
56.   z = ((zMSB << 8) | zLSB);
    
57. }
    
58. 
    
59. int setupL3G4200D(int scale){
    
60.   //From  Jim Lindblom of Sparkfun's code
    
61. 
    
62.   // Enable x, y, z and turn off power down:
    
63.   writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111);
    
64. 
    
65.   // If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2:
    
66.   writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000);
    
67. 
    
68.   // Configure CTRL_REG3 to generate data ready interrupt on INT2
    
69.   // No interrupts used on INT1, if you'd like to configure INT1
    
70.   // or INT2 otherwise, consult the datasheet:
    
71.   writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000);
    
72. 
    
73.   // CTRL_REG4 controls the full-scale range, among other things:
    
74. 
    
75.   if(scale == 250){
    
76.     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000);
    
77.   }else if(scale == 500){
    
78.     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000);
    
79.   }else{
    
80.     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000);
    
81.   }
    
82. 
    
83.   // CTRL_REG5 controls high-pass filtering of outputs, use it
    
84.   // if you'd like:
    
85.   writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000);
    
86. }
    
87. 
    
88. void writeRegister(int deviceAddress, byte address, byte val) {
    
89.     Wire.beginTransmission(deviceAddress); // start transmission to device
    
90.     Wire.write(address);       // send register address
    
91.     Wire.write(val);         // send value to write
    
92.     Wire.endTransmission();     // end transmission
    
93. }
    
94. 
    
95. int readRegister(int deviceAddress, byte address){
    
96. 
    
97.     int v;
    
98.     Wire.beginTransmission(deviceAddress);
    
99.     Wire.write(address); // register to read
    
100.     Wire.endTransmission();
     
101. 
     
102.     Wire.requestFrom(deviceAddress, 1); // read a byte
     
103. 
     
104.     v = Wire.read();
     
105.     return v;
     
106. }

複製代碼

g921002

姿態角的計算式是錯的~~歐耶~~

Bhai

姿態角的計算式是錯的~~歐耶~~
g921002 發表於 2012-11-21 22:15

請問能夠詳細解釋嗎?

Bhai

L3G4200D 的 X 乘上靈敏度，出來的結果就是 X 軸的角速度
得到角速度後再對時間積分，就能得到角度。

這是我在網路上查到的解釋，
請問是程式寫錯，還是理論錯了?

g921002

我們的物理空間是三維的吧?

Bhai

是。

抱歉，我不太能夠理解您的意思，
您所指的是我還少考慮什麼嗎?

siuling337

謝謝樓主無私的分享～！！！！