Ardoino L3G4200D 三軸數字陀螺儀測試

     L3G4200D陀螺儀有點貴，好像要130左右，我買了散件回來自己焊接的，居然一次焊成功了，以前重來沒焊過貼片工藝的板子，L3G4200D芯片太小了，不用鑷子都拿不出，回想一下焊接過程還真的是很驚險刺激。來張焊好的作品：

     

 

 其實這個板子和一個一元硬幣差不多大，焊接手法還不錯吧，哈。。

 

 和ANDRIOD接線方式如下：

 

 

測試代碼：

 

           #include <Wire.h> 
          
 
          
 
          #define CTRL_REG1 0x20 
          
 
          #define CTRL_REG2 0x21 
          
 
          #define CTRL_REG3 0x22 
          
 
          #define CTRL_REG4 0x23 
          
 
          #define CTRL_REG5 0x24 
          
 
          
 
          int L3G4200D_Address =  
          105;  
          // 
          I2C address of the L3G4200D 
          
  
          
 
          int x; 
          
 
          int y; 
          
 
          int z; 
          
 
          
 
          void setup(){ 
          
 
          
   Wire.begin(); 
          
   Serial.begin( 
          9600); 
          
 
          
   Serial.println( 
          " 
          starting up L3G4200D 
          "); 
          
   setupL3G4200D( 
          2000);  
          // 
           Configure L3G4200  - 250, 500 or 2000 deg/sec 
          
  
          
   delay( 
          1500);  
          // 
          wait for the sensor to be ready  
          
 } 
          
 
          
 
          void loop(){ 
          
   getGyroValues();   
          // 
           This will update x, y, and z with new values 
          
  
          
   Serial.print( 
          " 
          X: 
          "); 
          
   Serial.print(x); 
          
 
          
   Serial.print( 
          " 
           Y: 
          "); 
          
   Serial.print(y); 
          
 
          
   Serial.print( 
          " 
           Z: 
          "); 
          
   Serial.println(z); 
          
 
          
   delay( 
          100);  
          // 
          Just here to slow down the serial to make it more readable 
          
 } 
          
 
          
 
          void getGyroValues(){ 
          
 
          
    
          byte xMSB = readRegister(L3G4200D_Address,  
          0x29); 
          
    
          byte xLSB = readRegister(L3G4200D_Address,  
          0x28); 
          
   x = ((xMSB <<  
          8) | xLSB); 
          
 
          
    
          byte yMSB = readRegister(L3G4200D_Address,  
          0x2B); 
          
    
          byte yLSB = readRegister(L3G4200D_Address,  
          0x2A); 
          
   y = ((yMSB <<  
          8) | yLSB); 
          
 
          
    
          byte zMSB = readRegister(L3G4200D_Address,  
          0x2D); 
          
    
          byte zLSB = readRegister(L3G4200D_Address,  
          0x2C); 
          
   z = ((zMSB <<  
          8) | zLSB); 
          
 } 
          
 
          
 
          int setupL3G4200D( 
          int scale){ 
          
    
          // 
          From  Jim Lindblom of Sparkfun's code
 
    
          // 
           Enable x, y, z and turn off power down: 
          
   writeRegister(L3G4200D_Address, CTRL_REG1, 0b00001111); 
          
 
          
    
          // 
           If you'd like to adjust/use the HPF, you can edit the line below to configure CTRL_REG2: 
          
   writeRegister(L3G4200D_Address, CTRL_REG2, 0b00000000); 
          
 
          
    
          // 
           Configure CTRL_REG3 to generate data ready interrupt on INT2
    
          // 
           No interrupts used on INT1, if you'd like to configure INT1
    
          // 
           or INT2 otherwise, consult the datasheet: 
          
   writeRegister(L3G4200D_Address, CTRL_REG3, 0b00001000); 
          
 
          
    
          // 
           CTRL_REG4 controls the full-scale range, among other things: 
          
  
          
    
          if(scale ==  
          250){ 
          
     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00000000); 
          
   } 
          else  
          if(scale ==  
          500){ 
          
     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00010000); 
          
   } 
          else{ 
          
     writeRegister(L3G4200D_Address, CTRL_REG4, 0b00110000); 
          
   } 
          
 
          
    
          // 
           CTRL_REG5 controls high-pass filtering of outputs, use it
    
          // 
           if you'd like: 
          
   writeRegister(L3G4200D_Address, CTRL_REG5, 0b00000000); 
          
 } 
          
 
          
 
          void writeRegister( 
          int deviceAddress,  
          byte address,  
          byte val) { 
          
     Wire.beginTransmission(deviceAddress);  
          // 
           start transmission to device  
          
     Wire.write(address);        
          // 
           send register address 
          
     Wire.write(val);          
          // 
           send value to write 
          
     Wire.endTransmission();      
          // 
           end transmission 
          
 } 
          
 
          
 
          int readRegister( 
          int deviceAddress,  
          byte address){ 
          
 
          
      
          int v; 
          
     Wire.beginTransmission(deviceAddress); 
          
     Wire.write(address);  
          // 
           register to read 
          
     Wire.endTransmission(); 
          
 
          
     Wire.requestFrom(deviceAddress,  
          1);  
          // 
           read a byte 
          
  
          
      
          while(!Wire.available()) { 
          
          
          // 
           waiting 
          
     } 
          
 
          
     v = Wire.read(); 
          
      
          return v; 
          }  
         

 

 upload到andriod就可以通過COM口查看陀螺儀的測量結果了：

 

           starting up L3G4200D 
          
 X: 
          5 Y: 
          0 Z: 
          6 
          
 X: 
          6 Y: 
          1 Z: 
          6 
          
 X: 
          3 Y: 
          1 Z: 
          6 
          
 X: 
          0 Y: 
          2 Z: 
          7 
          
 X: 
          6 Y: 
          0 Z: 
          6 
          
 X: 
          3 Y: 
          0 Z: 
          10 
          
 X: 
          2 Y: 
          0 Z: 
          9 
          
 X: 
          1 Y: 
          0 Z: 
          7 
          
 X: 
          3 Y: 
          3 Z: 
          6 
          
 X: 
          1 Y: 
          0 Z: 
          9 
          
 X: 
          4 Y: 
          1 Z: 
          9 
          
 X: 
          9 Y: 
          1 Z: 
          8 
          
 X: 
          3 Y: 
          1 Z: 
          7 
          
 X: 
          6 Y: 
          1 Z: 
          6 
          
 X: 
          4 Y: 
          1 Z: 
          11 
          
 X: 
          8 Y: 
          0 Z: 
          10 
          
 X: 
          8 Y: 
          0 Z: 
          8 
          
 X: 
          6 Y: 
          1 Z: 
          11 
          
 X: 
          6 Y: 
          1 Z: 
          8 
          
 X: 
          8 Y: 
          1 Z: 
          11 
          
 X: 
          2 Y: 
          0 Z: 
          7 
          
 X: 
          1 Y: 
          1 Z: 
          10 
          
 X: 
          6 Y: 
          1 Z: 
          4 
          
 X: 
          7 Y: 
          1 Z: 
          9 
          
 X: 
          5 Y: 
          2 Z: 
          9 
          
 X: 
          4 Y: 
          9 Z:- 
          5 
          
 X: 
          14 Y:- 
          5 Z: 
          30 
          
 X: 
          4 Y: 
          0 Z: 
          7 
          
 X: 
          2 Y: 
          0 Z: 
          0 
          
 X: 
          8 Y: 
          0 Z: 
          8 
          
 X: 
          4 Y: 
          0 Z: 
          12 
          
 X: 
          3 Y: 
          1 Z: 
          9 
          
 X: 
          5 Y: 
          2 Z: 
          9 
          X:5 Y:0 Z:6  
         

 

 從結果看陀螺儀靜止狀態下數據還是有點跳動，這樣就需要濾波，我准備配合加速度傳感器利用卡爾曼濾波的方法來實現，目前尚在測試過程中。

 

    下面是陀螺儀靜止放在桌面上的Z軸角速度波形圖：

 

 

 陀螺儀運動中：

 

 

 

 陀螺儀數據處理

 

    陀螺儀的直接輸出值是相對靈敏軸的角速率，角速率對時間積分即可得到圍繞靈敏軸旋轉過的角度值。由於系統采用微控制器循環采樣程序獲取陀螺儀角速率信息，即每隔一段很短的時間采樣一次，所以采用累加的方法實現積分的功能來計算角度值： 

 

 

 

    公式中angle n為陀螺儀采樣到第n次的角度值；angle (n-1)為陀螺儀第n-1次采樣時的角度值；gyro n 為陀螺儀的第n次采樣得到的瞬時角速率值；dt為主運行一遍所用時間。可見，用陀螺儀輸出值積分計算角度，要求處理器運算速度足夠快，采樣程序應盡量簡練，程序循環一遍所用時間dt越小，采樣頻率越高，最后積分得到的角度值才能越精確。 

    陀螺儀是用來測量角速度信號的，通過對角速度積分，能得到角度值。但由於溫度變化、摩擦力和不穩定力矩等因素，陀螺儀會產成漂移誤差。而無論多么小的常值漂移通過積分都會得到無限大的角度誤差。因而，也不能單獨使用陀螺儀作為本機器人傾角傳感器。