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越小，採樣頻率越高，最後積分得到的角度值才能越精確。 

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